function [FitResult,Spectrum] = multi_comp_fit_v2(data_vol, EchoTimes, DecayMatrix, T2, Opt, varargin)
%
% multi_comp_fit_v2(data_file_name, EchoTimes, DecayMatrix, T2, Opt, ['ROI', roi_vol, 'tissue', mask_vol])
%
%**************************************************************************
% DESCRIPTION:
%
% A script to compute the multi-component T2 or T2* spectrum for an
% echo train data set using regularized (and unregulatrized) NNLS.
%
% If an ROI mask if given, the user will be prompted to proceed with a
% voxel-wise or ROI analysis.
%   voxel-wise analysis: The script will output superimposed plots of the T2/T2*
%                        spectrum for each voxel, and calculate the average
%                        MWF and gmT2/T2* for the ROI.
%   ROI analysis: The script will output plots of the average T2/T2* spectrum and
%                 the average raw data + fits for that ROI.
%
% If no mask is given then the script will output maps of the geometric
% mean T2/T2* (<T2/T2*>), the myelin water fraction (MWF) and the mono
% exponential T2/T2*.
%
% If a tissue classification mask is given (cls_file_name), the user will
% be prompted as to which tissue he/she wishes to analyze, and the maps
% will be created for that tissue only.
%
%**************************************************************************
% INPUTS:
%
%   * data_vol     = echo train data set to be analyzed
%   * EchoTimes    = vector with all echo times
%   * DecayMatrix  = matrix
%   * Opt          = struct that must contains:
%       1) RelaxationType   : 'T2' or 'T2star'
%       2) Sigma            : noise's sigma
%       3) lower_cutoff_MW  : lower cutoff on Myelin Water T2
%       4) upper_cutoff_MW  : upper cutoff on Myelin Water T2
%       5) upper_cutoff_IEW : upper cutoff on Intra/Extracellular Water T2
%   * 'ROI'        = ROI processing flag
%   * roi_vol      = ROI mask
%   * 'tissue'     = tissue processing flag
%   * mask_vol     = tissue mask, can be used to limit processing time
%   * varargin     = optional file limiting the number of echoes to be used
%                    for analysis. This can be:
%                  1) max_echoes.mnc file obtained from'in_plane_correction.m'
%                     to perform in-plane field inhomogeneity correction
%                  2) correction_mask.mnc, obtained from 'Gz_correction.m'.
%                     This binary file has voxels where the field gradient (Gz)
%                     was > 2mG/cm set to 0 (no field inhomog. correction performed*),
%                     and areas where Gz was < 2mG/cm set to 1.
%                     *The correction fails where Gz > 2mG/cm. In order to limit
%                     effect of field inhomogeneities in these voxels,
%                     processing is limited to 45 echoes, instead of 64.
%
%**************************************************************************
% EXAMPLE USES:
% multi_comp_fit_v2(EchoTimes, DecayMatrix, T2, Opt, 'tissue', cls_mask)
% --> Analyze vol T2 relaxation data, using a
% tissue classification mask: cls_mask.
%
% AUTHOR: Eva Alonso Ortiz (eva.alonso.ortiz@gmail.com)
% DATE LAST MODIFIED:
% March 2016 - WIP: complex data anaylis is not currently working, do not
% attempt to use it!
%
%**************************************************************************
% SPECIAL VERSION adapted for qMRLab
% By Ian Gagnon, 2017
% For the original version, see multi_comp_fit.m
% *************************************************************************

%%-------------------------------------------------------------------------
%% check existence of data files
%%-------------------------------------------------------------------------

% set processing labels
ROI_flag           = 0;
voxelwise_ROI_flag = 0;
tissue_flag        = 0;
mask_flag          = 0;

% default number of inputs
ndef_inputs = 5;

if nargin > ndef_inputs
    if nargin < ndef_inputs+3
        mask_opts = nargin - ndef_inputs;
    else
        mask_opts = nargin - ndef_inputs - 1;
    end

    for counter = 2:2:(mask_opts)
        switch varargin{counter-1}
            case{'ROI'}
                roi_vol = varargin{counter};
                ROI_flag = 1;
                voxelwise_ROI_flag = input('Voxel-wise ROI analysis (1) or average ROI analysis (0)?: ');
            case{'tissue'}
                mask_vol = varargin{counter};
                tissue_flag = 1;
        end
    end
end

%%-------------------------------------------------------------------------
%% Data informations
%%-------------------------------------------------------------------------

data_dim    = size(data_vol);
data_slices = data_dim(1,3);
data_height = data_dim(1,1);
data_width  = data_dim(1,2);
data_voxels = data_height*data_width;
num_echoes  = data_dim(1,4);

%%-------------------------------------------------------------------------
%% Compatibility mask/data
%%-------------------------------------------------------------------------

if ROI_flag == 1
    roi_voxels = size(roi_vol,1)*size(roi_vol,2);
    if roi_voxels ~= data_voxels
        error(sprintf('\nError in multi_comp_fit_v2: Mask file dimensions do not match data image file.\n'));
    end
end

if tissue_flag ~= 0
    mask_voxels = size(mask_vol,1)*size(mask_vol,2);
    if mask_voxels ~= data_voxels
        error(sprintf('\nError in multi_comp_fit_v2: Mask file dimensions do not match data image file.\n'));
    end
end

%%-------------------------------------------------------------------------
%% default values for NNLS fitting
%%-------------------------------------------------------------------------

T2.num = 120;

% set default values for reg-NNLS (taken from C. Chia)
if ~moxunit_util_platform_is_octave
    set(0,'RecursionLimit',5000)
end
mu = 0.25;
chi2range = [2 2.5];
chi2_min  = chi2range(1);
chi2_max  = chi2range(2);

%%-------------------------------------------------------------------------
%% apply ROI mask to data volumes
%%-------------------------------------------------------------------------

if (ROI_flag == 1 && voxelwise_ROI_flag == 0)
    mean_roi_data = get_avgROI_data(roi_vol, data_vol, data_dim);
end

%%-------------------------------------------------------------------------
%% NNLS fitting routine presets
%%-------------------------------------------------------------------------

% find cutoff indices
lower_cutoff_MW_index  = find_cutoff_index(Opt.lower_cutoff_MW, T2.vals);
upper_cutoff_MW_index  = find_cutoff_index(Opt.upper_cutoff_MW, T2.vals);
upper_cutoff_IEW_index = find_cutoff_index(Opt.upper_cutoff_IEW, T2.vals);

%%-------------------------------------------------------------------------
%% data fitting and analysis
%%-------------------------------------------------------------------------
if (ROI_flag == 1 && voxelwise_ROI_flag == 0)

    for slice = 1:data_slices

            %------------------------------
            % Do multi-exponential fitting
            %------------------------------

            % Do non-regularized NNLS
            [spectrum_NNLS(slice,:), chi2_NNLS(slice)] = do_NNLS(DecayMatrix, double(squeeze(mean_roi_data(slice,:))), Opt.Sigma(slice));

            ssq_res_NNLS(slice) = sum((DecayMatrix*squeeze(spectrum_NNLS(slice,:)') - squeeze(mean_roi_data(slice,:))').^2);
            s0_NNLS(slice)      = sum(spectrum_NNLS(slice,:));
            mwf_NNLS(slice)     = sum(spectrum_NNLS(slice,lower_cutoff_MW_index:upper_cutoff_MW_index))/s0_NNLS(slice);

            if isnan(ssq_res_NNLS(slice))
                ssq_res_NNLS(slice) = 0;
            end
            if isnan(s0_NNLS(slice))
                s0_NNLS(slice) = 0;
            end
            if isnan(mwf_NNLS(slice))
                mwf_NNLS(slice) = 0;
            end

            % Calculate the geometric mean of the T2 distribition for the non-reg NNLS
            gm_t2_NNLS(slice)     = exp(sum(squeeze(spectrum_NNLS(slice,:))'.*log(T2.vals))/sum(spectrum_NNLS(slice,:)));
            gm_IEW_t2_NNLS(slice) = exp(sum(squeeze(spectrum_NNLS(slice,upper_cutoff_MW_index:upper_cutoff_IEW_index))'.*log(T2.vals(upper_cutoff_MW_index:upper_cutoff_IEW_index)))/sum(spectrum_NNLS(slice,upper_cutoff_MW_index:upper_cutoff_IEW_index)));

            if isnan(gm_t2_NNLS(slice))
                gm_t2_NNLS(slice) = 0;
            end
            if isnan(gm_IEW_t2_NNLS(slice))
                gm_IEW_t2_NNLS(slice) = 0;
            end

            % Do regulaized NNLS
            [spectrum_regNNLS(slice,:), chi2_regNNLS(slice)] = ...
            iterate_NNLS(mu,chi2_min,chi2_max,T2.num,double(squeeze(mean_roi_data(slice,:))),DecayMatrix,chi2_NNLS(slice),Opt.Sigma(slice));

            ssq_res_regNNLS(slice) = sum((DecayMatrix*squeeze(spectrum_regNNLS(slice,:)') - squeeze(mean_roi_data(slice,:))').^2);
            s0_regNNLS(slice)      = sum(spectrum_regNNLS(slice,:));
            mwf_regNNLS(slice)     = sum(spectrum_regNNLS(slice,1:upper_cutoff_MW_index))/s0_regNNLS(slice);

            if isnan(ssq_res_regNNLS(slice))
                ssq_res_regNNLS(slice) = 0;
            end
            if isnan(s0_regNNLS(slice))
                s0_regNNLS(slice) = 0;
            end
            if isnan(mwf_regNNLS(slice))
                mwf_regNNLS(slice) = 0;
            end

            % Calculate the geometric mean of the T2 distribition for the reg NNLS
            gm_t2_regNNLS(slice)     = exp(sum(squeeze(spectrum_regNNLS(slice,:))'.*log(T2.vals))/sum(spectrum_regNNLS(slice,:)));
            gm_IEW_t2_regNNLS(slice) = exp(sum(squeeze(spectrum_regNNLS(slice,upper_cutoff_MW_index:upper_cutoff_IEW_index))'.*log(T2.vals(upper_cutoff_MW_index:upper_cutoff_IEW_index)))/sum(spectrum_regNNLS(slice,upper_cutoff_MW_index:upper_cutoff_IEW_index)));

            if isnan(gm_t2_regNNLS(slice))
                gm_t2_regNNLS(slice) = 0;
            end
            if isnan(gm_IEW_t2_regNNLS(slice))
                gm_IEW_t2_regNNLS(slice) = 0;
            end

            %-----------------------------
            % Do mono-exponential fitting
            %-----------------------------

            % Fit to single T2 decay component for comparison
            t2_guess = 100;
            guess = [t2_guess, mean_roi_data(slice,1)];

            [s0_fit(slice), t2_fit(slice)] = mono_exp_fit(double(squeeze(mean_roi_data(slice,:))), EchoTimes, double(guess));

            if isnan(s0_fit(slice))
                s0_fit(slice) = 0;
            end
            if isnan(s0_fit(slice))
                s0_fit(slice) = 0;
            end
    end

else
    if (ROI_flag == 1 && voxelwise_ROI_flag == 1)
        mask_flag = 1;
        mask_vol  = roi_vol;
    else
        if (tissue_flag == 0 && voxelwise_ROI_flag == 0)

            % create brain mask to limit processing time (and mask out noise in
            % images!)
%             create_brainmask(data_file_name);
            mask_flag = 1;
            % open brain mask
        else
            mask_flag = tissue_flag;
        end
    end

    % initialize all data vectors to zero
    spectrum_NNLS  = zeros(data_height,data_width,data_slices,T2.num);
    chi2_NNLS      = zeros(data_height,data_width,data_slices);
    gm_t2_NNLS     = zeros(data_height,data_width,data_slices);
    gm_IEW_t2_NNLS = zeros(data_height,data_width,data_slices);
    ssq_res_NNLS   = zeros(data_height,data_width,data_slices);
    s0_NNLS        = zeros(data_height,data_width,data_slices);
    mwf_NNLS       = zeros(data_height,data_width,data_slices);

    spectrum_regNNLS        = zeros(data_height,data_width,data_slices, T2.num);
    spectrum_regNNLS_A      = zeros(data_height,data_width,data_slices, T2.num);
    spectrum_regNNLS_B      = zeros(data_height,data_width,data_slices, T2.num);
    amp_spectrum_regNNLS    = zeros(data_height,data_width,data_slices, T2.num);
    phase_spectrum_regNNLS  = zeros(data_height,data_width,data_slices, T2.num);
    chi2_regNNLS            = zeros(data_height,data_width,data_slices);
    gm_t2_regNNLS           = zeros(data_height,data_width,data_slices);
    gm_IEW_t2_regNNLS       = zeros(data_height,data_width,data_slices);
    gm_MW_t2_regNNLS        = zeros(data_height,data_width,data_slices);
    ssq_res_regNNLS         = zeros(data_height,data_width,data_slices);
    s0_regNNLS              = zeros(data_height,data_width,data_slices);
    mwf_regNNLS             = zeros(data_height,data_width,data_slices);
    mw_delf_regNNLS         = zeros(data_height,data_width,data_slices);

    s0_fit = zeros(data_height,data_width,data_slices);
    t2_fit = zeros(data_height,data_width,data_slices);

    if (ROI_flag == 1 && voxelwise_ROI_flag == 1)

        %-------------------------------------------
        % Prepare figure for superimposed spectrum
        %-------------------------------------------
        figure; hold on;
        set(gca,'xscale','log','FontSize',20,'XMinorTick','on')
        xlim([T2.range(1) T2.range(2)])
        ylabel('Normalized Signal');
        if strcmp(Opt.RelaxationType,'T2')
            xlabel('T2 (ms)');
            title('T_2 Spectrum','FontSize',20);
        elseif strcmp(Opt.RelaxationType,'T2star')
            xlabel('T2* (ms)');
            title('T_2^* Spectrum','FontSize',20);
        end
        x = [Opt.upper_cutoff_MW, Opt.upper_cutoff_MW];
        y = [0, 1];
        plot(x,y,'r-','LineWidth', 4);
        grid minor;

    end

    for slice = 1:data_slices

        for i = 1:data_height
            for j = 1:data_width

                % only fill in what corresponds to the mask
                if mask_vol(i,j,slice) == mask_flag
                    this_echo_times             = EchoTimes;
                    this_num_echoes             = num_echoes;
                    this_DecayMatrix            = DecayMatrix;
                    this_lower_cutoff_MW_index  = lower_cutoff_MW_index;
                    this_upper_cutoff_MW_index  = upper_cutoff_MW_index;
                    this_upper_cutoff_IEW_index = upper_cutoff_IEW_index;

                    % Do non-regularized NNLS
                    [spectrum_NNLS(i,j,slice,:), chi2_NNLS(i,j,slice)] = do_NNLS(this_DecayMatrix, double(squeeze(data_vol(i,j,slice,1:this_num_echoes)))', Opt.Sigma(slice));

                    % Do regulaized NNLS
                    [spectrum_regNNLS(i,j,slice,:), chi2_regNNLS(i,j,slice)] = ...
                    iterate_NNLS(mu,chi2_min,chi2_max,double(squeeze(data_vol(i,j,slice,1:this_num_echoes)))',this_DecayMatrix,chi2_NNLS(i,j,slice),Opt.Sigma(slice));
                    %------------------------------
                    % Do multi-exponential fitting
                    %------------------------------

                    s0_regNNLS(i,j,slice)  = sum(spectrum_regNNLS(i,j,slice,:));
                    mwf_regNNLS(i,j,slice) = sum(spectrum_regNNLS(i,j,slice,1:this_upper_cutoff_MW_index))/s0_regNNLS(i,j,slice);

                    % Calculate the geometric mean of the T2 distribition for the reg NNLS
                    gm_t2_regNNLS(i,j,slice)     = exp(sum(squeeze(spectrum_regNNLS(i,j,slice,:)).*log(T2.vals))/sum(spectrum_regNNLS(i,j,slice,:)));
                    gm_IEW_t2_regNNLS(i,j,slice) = exp(sum(squeeze(spectrum_regNNLS(i,j,slice,this_upper_cutoff_MW_index:this_upper_cutoff_IEW_index)).*log(T2.vals(this_upper_cutoff_MW_index:this_upper_cutoff_IEW_index)))/sum(spectrum_regNNLS(i,j,slice,this_upper_cutoff_MW_index:this_upper_cutoff_IEW_index)));
                    gm_MW_t2_regNNLS(i,j,slice)  = exp(sum(squeeze(spectrum_regNNLS(i,j,slice,this_lower_cutoff_MW_index:this_upper_cutoff_MW_index)).*log(T2.vals(this_lower_cutoff_MW_index:this_upper_cutoff_MW_index)))/sum(spectrum_regNNLS(i,j,slice,this_lower_cutoff_MW_index:this_upper_cutoff_MW_index)));

                    if isnan(gm_t2_regNNLS(i,j,slice))
                        gm_t2_regNNLS(i,j,slice) = 0;
                    end
                    if isnan(gm_IEW_t2_regNNLS(i,j,slice))
                        gm_IEW_t2_regNNLS(i,j,slice) = 0;
                    end
                    if isnan(gm_MW_t2_regNNLS(i,j,slice))
                        gm_MW_t2_regNNLS(i,j,slice) = 0;
                    end
                    if isnan(s0_regNNLS(i,j,slice))
                        s0_regNNLS(i,j,slice) = 0;
                    end
                    if isnan(mwf_regNNLS(i,j,slice))
                        mwf_regNNLS(i,j,slice) = 0;
                    end

                    %-----------------------------
                    % Do mono-exponential fitting
                    %-----------------------------

                    if (ROI_flag == 1 && voxelwise_ROI_flag == 1)

                        %-----------------------------
                        %  Plot the T2 spectrum
                        %-----------------------------

                        % Normalize distributions
                        norm_spectrum_NNLS(i,j,slice,:)    = spectrum_NNLS(i,j,slice,:)/max(spectrum_NNLS(i,j,slice,:));
                        norm_spectrum_regNNLS(i,j,slice,:) = spectrum_regNNLS(i,j,slice,:)/max(spectrum_regNNLS(i,j,slice,:));

                        plot(T2.vals,squeeze(norm_spectrum_regNNLS(i,j,slice,:)), 'b-','LineWidth',0.1);
                        print('-djpeg','superimposed_spectrum.jpeg');

                    end
                end
            end
        end
    end
end

%--------------------------------------------------------------------------

if ( ROI_flag == 1 && voxelwise_ROI_flag == 0 )

    for slice = 1:data_slices

        %% Plot the T2 spectrum

        % Normalize distributions
        norm_spectrum_NNLS(slice,:)    = spectrum_NNLS(slice,:)/max(spectrum_NNLS(slice,:));
        norm_spectrum_regNNLS(slice,:) = spectrum_regNNLS(slice,:)/max(spectrum_regNNLS(slice,:));

        figure; hold on;
        grid minor;
        %bar1 = bar(T2.vals,norm_spectrum_NNLS(slice,:),'FaceColor', 'g', 'EdgeColor', 'g','LineWidth',1);
        %set(bar1,'BarWidth',0.1);

%         %--------------------
%         % If the data is simulated: Plot "true" spectrum (otherwise,
%         % comment this out)
%         %--------------------
%         T2 = [5 50]';
%         frac = [0.12 0.88]';
%
%         true_spectrum = zeros(size(T2.vals));
%         for j = 1: size(T2)
%             for i = 1:(size(T2.vals)-1)
%                 if i == 1
%                     if T2(j) == T2.vals(i)
%                         true_spectrum(i) = frac(j);
%                     end
%                 end
%
%                 if ((T2.vals(i+1) >= T2(j)) && (T2.vals(i) <= T2(j)))
%                     true_spectrum(i) = frac(j);
%                 end
%             end
%         end
%         bar2 = bar(T2.vals,true_spectrum(:),'FaceColor', 'm', 'EdgeColor', 'm','LineWidth',1);
%         %set(bar2,'BarWidth',0.1);
%         %--------------------

        plot(T2.vals,norm_spectrum_regNNLS(slice,:), 'b-','LineWidth',1.5);
        set(gca,'xscale','log','FontSize',20)
        xlim([T2.range(1) T2.range(2)])
        %xlim([0.002*1e3 4*1e3]) %keep T2* and T2 ranges the same for plot
        ylabel('Normalized Signal');
        if strcmp(Opt.RelaxationType,'T2')
            xlabel('T2 (ms)');
            title('T_2 Spectrum','FontSize',20);
        elseif strcmp(Opt.RelaxationType,'T2star')
            xlabel('T2* (ms)');
            title('T_2^* Spectrum','FontSize',20);
        end
        x = [Opt.lower_cutoff_MW, Opt.upper_cutoff_MW];
        y = [0, 1];
        plot(x,y,'r-','LineWidth', 2);
%         legend('location', 'NorthEast','NNLS multi-exp fit','True Spectrum','regularized NNLS multi-exp fit')
%         legend('location', 'NorthEast','NNLS multi-exp fit','regularized NNLS multi-exp fit')
%         legend('location', 'NorthEast','regularized NNLS multi-exp fit')
        print('-djpeg','spectrum.jpeg')

        %% Plot the decay curve and fitted line

        % Reconstruct the signal intensities and plot back on
        % top of the original data.

        single_sig_recon(slice,:)    = s0_fit(slice).*exp(-EchoTimes/t2_fit(slice));
        multi_sig_recon(slice,:)     = DecayMatrix * spectrum_NNLS(slice,:)';
        reg_multi_sig_recon(slice,:) = DecayMatrix * spectrum_regNNLS(slice,:)';

        figure; hold on;
        plot(EchoTimes, mean_roi_data(slice,:), 'kx', 'MarkerSize',10);
%         plot(echo_times, single_sig_recon(slice,:), 'g-.','LineWidth',2);
%         plot(echo_times, multi_sig_recon(slice,:), 'b-','LineWidth',2);
        plot(EchoTimes, reg_multi_sig_recon(slice,:), 'r--','LineWidth',2);
        if strcmp(Opt.RelaxationType,'T2')
%             legend('location', 'best', 'SE data','mono-exponential fit','NNLS multi-exp fit','regularized NNLS multi-exp fit');
            legend('location', 'best', 'SE data','regularized NNLS multi-exp fit');
        elseif strcmp(Opt.RelaxationType,'T2star')
%             legend('location', 'best','GRE data','mono-exponential fit','NNLS multi-exp fit','regularized NNLS multi-exp fit');
            legend('location', 'best', 'SE data','regularized NNLS multi-exp fit');
        end
        xlabel('TE (ms)');
        ylabel('Signal (arb)');
        grid on;
        print('-djpeg','signal_decay.jpeg');

        if strcmp(Opt.RelaxationType,'T2')
            %disp(fprintf('\nmulti-exp NNLS (slice %d) => <T2>: %3.2f ms, MWF:  %3.2f', slice, gm_t2_NNLS(slice), mwf_NNLS(slice)));
            disp(fprintf('multi-exp regNNLS (slice %d) => IE water <T2>: %3.2f ms, MWF:  %3.2f', slice, gm_IEW_t2_regNNLS(slice), 100*mwf_regNNLS(slice)));
            %disp(fprintf('Mono-exponential T2 fit (slice %d): %3.2f ms', slice, t2_fit(slice)));
        elseif strcmp(Opt.RelaxationType,'T2star')
            %disp(fprintf('\nmulti-exp NNLS (slice %d) => <T2*>: %3.2f ms, MWF:  %3.2f', slice, gm_t2_NNLS(slice), mwf_NNLS(slice)));
            disp(fprintf('multi-exp regNNLS (slice %d) => IE water <T2*>: %3.2f ms, MWF:  %3.2f', slice, gm_IEW_t2_regNNLS(slice), 100*mwf_regNNLS(slice)));
            %disp(fprintf('Mono-exponential T2* fit (slice %d): %3.2f ms', slice, t2_fit(slice)));
        end

   end

elseif ROI_flag == 0
    FitResult.MWF   = 100*mwf_regNNLS;
    FitResult.T2IEW = gm_IEW_t2_regNNLS;
    FitResult.T2MW  = gm_MW_t2_regNNLS;
    Spectrum        = squeeze(spectrum_regNNLS);
end

if ( tissue_flag ~= 0 || voxelwise_ROI_flag == 1 )
    mwf_regNNLS       = reshape(mwf_regNNLS,data_voxels,data_slices);
    gm_IEW_t2_regNNLS = reshape(gm_IEW_t2_regNNLS,data_voxels,data_slices);
    for slice = 1:data_slices
        mROI_mwf_regNNLS(slice)         = mean(nonzeros(mwf_regNNLS(:,slice)));
        valtmp           = std(nonzeros(mwf_regNNLS(:,slice)));
        if isempty(valtmp), stdROI_regNNLS(slice) = nan; else stdROI_regNNLS(slice) = valtmp; end
        mROI_gm_IEW_t2_regNNLS(slice)   = mean(nonzeros(gm_IEW_t2_regNNLS(:,slice)));
        valtmp = std(nonzeros(gm_IEW_t2_regNNLS(:,slice)));
        if isempty(valtmp), stdROI_gm_IEW_t2_regNNLS(slice) = nan; else stdROI_gm_IEW_t2_regNNLS(slice) = valtmp; end

%         mROI_gm_MW_t2_regNNLS(slice) = mean(nonzeros(gm_MW_t2_regNNLS(:,slice)));
%         stdROI_gm_MW_t2_regNNLS(slice) = std(nonzeros(gm_MW_t2_regNNLS(:,slice)));
%         disp(fprintf('\nAverage regMWF over tissue mask (slice %d): %3.2f +/- %3.2f %%', slice, 100*mROI_mwf_regNNLS(slice), 100*stdROI_regNNLS(slice)));
%         disp(fprintf('\nAverage IE water gm_T2 over tissue mask (slice %d): %3.2f +/- %3.2f ms', slice, mROI_gm_IEW_t2_regNNLS(slice), stdROI_gm_IEW_t2_regNNLS(slice)));
%         disp(fprintf('\nAverage Myelin water gm_T2 over tissue mask (slice %d): %3.2f +/- %3.2f ms', slice, mROI_gm_MW_t2_regNNLS(slice), stdROI_gm_MW_t2_regNNLS(slice)));
    end
end

%--------------------------------------------------------------------------


function [FF,II] = remesh_at_points(V,F,P)
  % REMESH_AT_POINTS Given a surface mesh (V,F) and a list of points on/near the
  % surface P, subdivide triangles of (V,F) so that P are now contained in the
  % vertex set.
  %
  % Inputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of face indices into V
  %   P  #P by 3 list of point positions
  % Outputs:
  %   FF  #FF by 3 list of face indices into [V;P]
  %   II  #FF list of indices into F revealing birth face
  %
  % Example:
  %   P = random_points_on_mesh(V,F,size(F,1));
  %   [FF,II] = remesh_at_points(V,F,P);
  %   tsurf(FF,[V;P],'CData',II)
  %

  VV = V;
  FF = F;
  II = (1:size(F,1))';
  JJ = zeros(size(V,1),1);
  L = (1:size(P,1))';
  PP = P;
  while true
    %clf;hold on;tsurf(FF,VV,'CData',II);scatter3(PP(:,1),PP(:,2),PP(:,3));hold off;pause
    % Find closest points/faces on mesh (This is a bit redundant. Really we
    % should only look at faces that _were_ affected last round, since those
    % must have had multiple points.)
    [~,IC,C] = point_mesh_squared_distance(PP,VV,FF);
    % Only keep one closest point per face
    [I,J] = unique(IC);
    % append keepers
    n = size(VV,1);
    JJ = [JJ;L(J)];
    VV = [VV;C(J,:)];
    nCI = n+(1:numel(J))';
    unaffected = setdiff(1:size(FF,1),I);
    II = [II(unaffected);repmat(II(I),3,1)];
    FF = [ ...
      FF(unaffected,:);
      FF(I,[1 2]) nCI; ...
      FF(I,[2 3]) nCI; ...
      FF(I,[3 1]) nCI];
    % Leftovers
    L = L(setdiff(1:end,J));
    PP = P(L,:);
    if isempty(PP)
      break;
    end
  end

  %% Re-order so that output is simply VV = [V;C]
  %C = zeros(size(P));
  %C(JJ(size(V,1)+1:end),:) = VV(size(V,1)+1:end,:);
  I = JJ+size(V,1);
  I(1:size(V,1)) = 1:size(V,1);
  FF = I(FF);
  %VV = [V;C];
end


function [newpt elemid weight]=proj2mesh(v,f,pt,nv,cn,radmax)
%  [newpt elemid weight]=proj2mesh(v,f,pt,nv,cn)
%
%  project a point cloud on to the surface mesh (surface can only be triangular)
%
%  author: Qianqian Fang <q.fang at neu.edu>
%  date: 12/12/2008
%
% parameters:
%      v: node coordinate of the surface mesh (nn x 3)
%      f: element list of the surface mesh (3 columns for
%            triangular mesh, 4 columns for cubic surface mesh)
%      pt: points to be projected, 3 columns for x,y and z respectively
%      nv: nodal norms (vector) calculated from nodesurfnorm.m
%          with dimensions of (size(v,1),3)
%      cn: a integer vector with the length of p, denoting the closest
%          surface nodes (indices of v) for each point in p. this
%          value can be calculated from dist2surf.m
%      radmax: if speicified, the search for elements to project will be
%          limited to those within a bounding box with half-edge-length
%          of radmax centered at the point to be projected
%
%      if nv and cn are not supplied, proj2mesh will project the point
%      cloud onto the surface by the direction pointing to the centroid
%      of the mesh
%
% outputs:
%      newpt: the projected points from p
%      elemid: a vector of length of p, denotes which surface trangle (in elem)
%             contains the projected point
%      weight: the barycentric coordinate for each projected points, these are
%             the weights
%
% Please find more information at http://iso2mesh.sf.net/cgi-bin/index.cgi?metch
%
% this function is part of "metch" toobox, see COPYING for license

cent=mean(v);
enum=length(f);
ec=reshape(v(f(:,1:3)',:)', [3 3,enum]);
centroid=squeeze(mean(ec,2));
newpt =zeros(size(pt,1),3);
elemid=zeros(size(pt,1),1);
weight=zeros(size(pt,1),3);
[idoldmesh,loc]=ismember(pt,v,'rows');
idnode=find(idoldmesh);
if(~isempty(idnode))
    [tt,ll]=ismember(loc(idnode),f);
    [p1,p2]=ind2sub(size(f),ll); % p1 is the index in f
    newpt(idnode,:)=pt(idnode,:);
    elemid(idnode)=p1;
    weight(sub2ind(size(weight),idnode,p2))=1;
end
radlimit=-1;

if(nargin>=5)
       % if nv and cn are supplied, use nodal norms to project the points
       direction=nv(cn,:);
       if(nargin>=6)
           radlimit=radmax;
       end
elseif(nargin==3)
       % otherwise, project toward the centroid
       direction=pt-repmat(cent,size(pt,1),1);
end

for t=1:size(pt,1)
    if(idoldmesh(t)~=0) continue; end
    maxdist=sqrt(sum((pt(t,:)-cent).*(pt(t,:)-cent)));

    if(radlimit>0)
        maxdist=radlimit;
    end

    idx=find(centroid(1,:)>pt(t,1)-maxdist & ...
             centroid(1,:)<pt(t,1)+maxdist & ...
             centroid(2,:)>pt(t,2)-maxdist & ...
             centroid(2,:)<pt(t,2)+maxdist & ...
             centroid(3,:)>pt(t,3)-maxdist & ...
             centroid(3,:)<pt(t,3)+maxdist );

    dist=centroid(:,idx);
    dist(1,:)=dist(1,:)-pt(t,1);
    dist(2,:)=dist(2,:)-pt(t,2);
    dist(3,:)=dist(3,:)-pt(t,3);
    c0=sum(dist.*dist);

    % sort the distances to accelate the calculation
    [c1,sorted]=sort(c0);

    for i=1:length(idx)

        % project the point along vector direction and calculate the intersection to a plane

        [inside,p,w]=linextriangle(pt(t,:), pt(t,:)+direction(t,:),v(f(idx(sorted(i)),:),:));

	% the intersection is within the current trianglar surface
        if(inside)
            newpt(t,:)=p;
            weight(t,:)=w;
            elemid(t)=idx(sorted(i));
            break;
        end
    end
end


function headmodel = ft_headmodel_duneuro(mesh, varargin)

% FT_HEADMODEL_DUNEURO creates a volume conduction model of the head
% using the finite element method (FEM) for EEG and MEG. Different source models
% are implemented, including the St. Venant, the subtraction and partial
% integration model. This function takes as input a mesh with tetrahedral
% or hexahedral elements and corresponding conductivities and returns
% as output a volume conduction model which can be used to compute EEG/MEG
% leadfields.
%
% Use as
%   headmodel = ft_headmodel_duneuro(mesh,'conductivity', conductivities, ...)
%   headmodel = ft_headmodel_duneuro(mesh,'grid_filename', grid_filename, 'tensors_filename', tensors_filename, ...)
%
% Required input arguments should be specified in key-value pairs and have
% to include either
%   grid_filename   = string, filename for grid in "msh" fileformat (see here: https://gmsh.info/doc/texinfo/gmsh.html#File-formats)
%   tensors_filename= string, filename for conductivities, txt file with conductivity values
%
% or
%   conductivity    = vector, conductivity values for tissues
%
% Optional input arguments are passed with
%   duneuro_settings = (optional) struct, which can contain the following fields
%
%   type            = string, 'fitted' (default)
%   solver_type     = string, 'cg' (default)
%   electrodes      = string, 'closest_subentity_center' (default)
%   subentities     = string, e.g. '1 2 3' (default) or '3'
%   forward         = string, 'venant' (default), 'partial_integration'
%   intorderadd     = string, e.g. '2' (default)
%   intorderadd_lb  = string, e.g. '2' (default)
%   initialization  = string, e.g. 'closest_vertex' (default)
%   numberOfMoments = string, e.g. '3' (default)
%   referenceLength = string, e.g. '20' (default)
%   relaxationFactor= string, e.g. '1e-6' (default)
%   restrict        = string, e.g. 'true' (default)
%   weightingExponent= string, e.g. '1' (default)
%   post_process    = string, e.g. 'true' (default)
%   subtract_mean   = string, e.g. 'true' (default)
%   reduction       = string, e.g. '1e-10' (default)
%   intorderadd_meg = integer, e.g.'0' (default)
%   mixedMoments    = logical, e.g. 'true' (default)
%   meg_type        = string, e.g. 'physical' (default)
%   meg_eneablecache= logical, e.g. 'false (default)

ft_hastoolbox('duneuro', 1);

grid_filename   = ft_getopt(varargin, 'grid_filename');
tensors_filename= ft_getopt(varargin, 'tensors_filename');
conductivity    = ft_getopt(varargin, 'conductivity');
duneuro_settings = ft_getopt(varargin, 'duneuro_settings');

% get the optional arguments and set defaults
if(isfield(duneuro_settings, 'type')) type = duneuro_settings.type;
else type = 'fitted';
end

if(isfield(duneuro_settings, 'solver_type')) solver_type = duneuro_settings.solver_type;
else solver_type = 'cg';
end

if(isfield(duneuro_settings, 'electrodes')) electrodes = duneuro_settings.electrodes;
else electrodes = 'closest_subentity_center';
end

if(isfield(duneuro_settings, 'subentities')) subentities = duneuro_settings.subentities;
else subentities = '1 2 3';
end

if(isfield(duneuro_settings, 'forward')) forward = duneuro_settings.forward;
else forward = 'venant';
end

if(isfield(duneuro_settings, 'intorderadd')) intorderadd = duneuro_settings.intorderadd;
else intorderadd = '2';
end

if(isfield(duneuro_settings, 'intorderadd_lb')) intorderadd_lb = duneuro_settings.intorderadd_lb;
else intorderadd_lb = '2';
end

if(isfield(duneuro_settings, 'initialization')) initialization = duneuro_settings.initialization;
else initialization = 'closest_vertex';
end

if(isfield(duneuro_settings, 'numberOfMoments')) numberOfMoments = duneuro_settings.numberOfMoments;
else numberOfMoments = '3';
end

if(isfield(duneuro_settings, 'referenceLength')) referenceLength = duneuro_settings.referenceLength;
else referenceLength = '20';
end

if(isfield(duneuro_settings, 'relaxationFactor')) relaxationFactor = duneuro_settings.relaxationFactor;
else relaxationFactor = '1e-6';
end

if(isfield(duneuro_settings, 'restrict')) restrict = duneuro_settings.restrict;
else restrict = 'true';
end

if(isfield(duneuro_settings, 'weightingExponent')) weightingExponent = duneuro_settings.weightingExponent;
else weightingExponent = '1';
end

if(isfield(duneuro_settings, 'post_process')) post_process = duneuro_settings.post_process;
else post_process = 'true';
end

if(isfield(duneuro_settings, 'subtract_mean')) subtract_mean = duneuro_settings.subtract_mean;
else subtract_mean = 'true';
end

if(isfield(duneuro_settings, 'reduction')) reduction = duneuro_settings.reduction;
else reduction = '1e-15';
end

if(isfield(duneuro_settings, 'intorderadd_meg')) intorderadd_meg = duneuro_settings.intorderadd_meg;
else intorderadd_meg = '2';
end

if(isfield(duneuro_settings, 'mixedMoments')) mixedMoments = duneuro_settings.mixedMoments;
else mixedMoments = 'true';
end

if(isfield(duneuro_settings, 'meg_type')) reduction = duneuro_settings.meg_type;
else meg_type = 'physical';
end

if(isfield(duneuro_settings, 'meg_eneablecache')) meg_eneablecache = duneuro_settings.meg_eneablecache;
else meg_eneablecache = 'false';
end

% start with an empty volume conductor
mesh = ft_datatype_parcellation(mesh);
headmodel = [];
if isfield(mesh,'pos')
  headmodel.pos = mesh.pos;
else
  error('Vertex field is required!')
end

if isfield(mesh,'tet')
  headmodel.tet = mesh.tet;
elseif isfield(mesh,'hex')
  headmodel.hex = mesh.hex;
  %%assume fieldtrip ordering, reorder to conform to DUNE standard
  headmodel.hex(:,3) = mesh.hex(:,4);
  headmodel.hex(:,4) = mesh.hex(:,3);
  headmodel.hex(:,7) = mesh.hex(:,8);
  headmodel.hex(:,8) = mesh.hex(:,7);
else
  error('Element field with tetrahedral or hexahedral information is required!')
end

if isfield(mesh,'tissue')
  headmodel.tissue = mesh.tissue;
else
  error('No tissue labels declared!')
end

if (isempty(grid_filename) || isempty(tensors_filename)) && (isempty(conductivity))
  error('Either a filename for the grid and a filename for the conductivity or a conductivity array must be supplied')
end

if ~isempty(conductivity)
  if (size(conductivity,1) ~= 1 && size(conductivity,1) ~= 9)  || size(conductivity,2) ~= length(unique(headmodel.tissue))
    error('Conductivity has wrong dimension!')
  end
end

if ~isfield(mesh,'tissuelabel')
  numlabels = size(unique(mesh.tissue),1);
  headmodel.tissuelabel = {};
  ulabel = unique(labels);
  for i = 1:numlabels
    headmodel.tissuelabel{i} = num2str(ulabel(i));
  end
else
  headmodel.tissuelabel = mesh.tissuelabel;
end

% create driver object
cfg                 = [];
cfg.type            = type;
cfg.solver_type     = solver_type;
cfg.meg.intorderadd = intorderadd_meg;
cfg.meg.type        = meg_type;
cfg.meg.cache.enable = meg_eneablecache;

if isfield(mesh,'tet')
  cfg.element_type = 'tetrahedron';
elseif isfield(mesh,'hex')
  cfg.element_type = 'hexahedron';
end

if(~isempty(grid_filename) && (~isempty(tensors_filename)))
  cfg.volume_conductor.grid.filename = grid_filename;
  cfg.volume_conductor.tensors.filename = tensors_filename;
else
  if isfield(headmodel,'tet')
    cfg.volume_conductor.grid.elements = (uint64(headmodel.tet))' - 1;
  elseif isfield(headmodel,'hex')
    cfg.volume_conductor.grid.elements = (uint64(headmodel.hex))' - 1;
  end
  cfg.volume_conductor.grid.nodes = headmodel.pos';

  %make sure labels start at 0 %TODO: faster for 6C anisotropy
  utissue = sort(unique(headmodel.tissue));
  for i = 1:size(headmodel.tissue,1)
    headmodel.tissue(i,1) =  find(utissue == headmodel.tissue(i,1));
  end
  cfg.volume_conductor.tensors.labels = uint64(headmodel.tissue -1);
  if(size(conductivity,1) == 1)
    cfg.volume_conductor.tensors.conductivities = conductivity;
  elseif (size(conductivity,1) == 9)
    cfg.volume_conductor.tensors.tensors = conductivity;
  end
end

try
    headmodel.driver = duneuro_meeg(cfg);
catch
    warning('To generate a valid Mex File, please compile duneuro-matlab on your machine following the instructions on the DUNEuro Wiki page.');
    return
end

headmodel.type              = 'duneuro';
headmodel.forward           = forward;
headmodel.electrodes        = electrodes;
headmodel.subentities       = subentities;
headmodel.intorderadd       = intorderadd;
headmodel.intorderadd_lb    = intorderadd_lb;
headmodel.initialization    = initialization;
headmodel.numberOfMoments   = numberOfMoments;
headmodel.referenceLength   = referenceLength;
headmodel.relaxationFactor  = relaxationFactor;
headmodel.restrict          = restrict;
headmodel.weightingExponent = weightingExponent;
headmodel.mixedMoments      = mixedMoments;
headmodel.post_process      = post_process;
headmodel.subtract_mean     = subtract_mean;
headmodel.reduction         = reduction;

end


function [y,ev] = dmrg_eigb(a,k,eps,varargin)
%Find several minimal eigenvalues of a TT-matrix using DMRG method
%   [Y,EV]=DMRG_EIGB(A,K,EPS,OPTIONS) Attempts to find K minimal
%   eigenvalues of a TT-matrix A with accuracy EPS. We use minimization of
%   Block-Rayleigh quotient to do this. The solution is returned a block
%   TT-tensor (i.e, r(d+1) is equal to K).
%   Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o y0 - initial approximation [random rank-5 tensor]
%       o rmax - maximal  TT-rank of the (block) solution [2500]
%       o nswp - maximal number of sweeps [4]
%       o kick_rank - stabilization parameter, the larger, the better
%       accuracy but the higher complexity [5]
%       o verb - print debug info [ {true} | false ]
%       o msize - the size of local matrix when the iterative solver is
%       used
%   Example:
%       d=8; f=3;
%       mat=tt_qlaplace_dd(d*ones(1,f)); %Laplace in the QTT-format
%       [v,ev]=dmrg_eigb(mat,5,1e-6); %5 lowest eigenvalues
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

warning('This realisation is old and not optimal. Consider using dmrg_eig');

%Default parameters
y0=[];
nswp=4;
msize=1;
max_l_steps=200;
kickrank=4;
verb=1;
rmax = Inf;
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'kickrank'
            kickrank=lower(varargin{i+1});
        case 'x0'
            y0=varargin{i+1};
        case 'msize'
            msize=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});

    end
end

n=a.n;
d=a.d;
if ( isempty(y0) )
    kk=max(5,k);
    r=kk*ones(1,d+1);
    r(d+1)=k;
    r(1)=1;
    y0=tt_random(n,d,r);
end
y=round(y0,0);

local_eps = eps/sqrt(d);

%We start from the orthogonalization of the y vector from left-to-right
%(it does not influence the TT-ranks)

psy=y.ps;
ry=y.r;
tta=a.tt;
psa=tta.ps;
ra=tta.r;
cra=tta.core;
cry=y.core;
rm=1;
%Also we will need to compute phi-matrices
phi=cell(d+1,1);
phi{1}=1; %This one is right now unchanged
phi{d+1}=1; %Seems to be unchanged also
pos1=1;
for i=1:d
    cr=cry(psy(i):psy(i+1)-1);
    cr=reshape(cr,[ry(i),n(i)*ry(i+1)]);
    cr=rm*cr;
    ry(i)=size(cr,1);
    cr=reshape(cr,[ry(i)*n(i),ry(i+1)]);
    [u,rm]=qr(cr,0);
    rnew=size(u,2);
    %ry(i+1)=rnew;
    cry(pos1:pos1+ry(i)*n(i)*rnew-1)=u(:);
    pos1=pos1+ry(i)*n(i)*rnew;
    %With this new core compute (Ax,x) to the phi matrix
    %u=reshape(u,[ry(i),n(i),ry(i+1)]);
    crm=cra(psa(i):psa(i+1)-1);
    crm=reshape(crm,[ra(i),n(i),n(i),ra(i+1)]);
    phx=phi{i};
    phx=reshape(phx,[ra(i),ry(i),ry(i)]); phx=permute(phx,[1,3,2]);
    x0=u;
    x0=reshape(x0,[ry(i),n(i)*rnew]);
    phx=reshape(phx,[ra(i)*ry(i),ry(i)]);
    phx=phx*x0; %phx is ra(i)*ry(i)*n(i)*ry(i+1)
    phx=reshape(phx,[ra(i),ry(i),n(i),rnew]);
    crm=permute(crm,[1,3,2,4]); crm=reshape(crm,[ra(i)*n(i),n(i)*ra(i+1)]);
    %Convolve over (ak-1) j with the matrix
    phx=permute(phx,[2,4,1,3]);
    phx=reshape(phx,[ry(i)*rnew,ra(i)*n(i)]);
    phx=phx*crm; %ry(i)*ry(i+1)*n(i)*ra(i+1)
    phx=reshape(phx,[ry(i),rnew,n(i),ra(i+1)]);
    phx=permute(phx,[2,4,1,3]);
    phx=reshape(phx,[rnew*ra(i+1),ry(i)*n(i)]);
    x0=u;
    x0=reshape(x0,[ry(i)*n(i),rnew]);
    phx=phx*x0;
    phx=reshape(phx,[rnew,ra(i+1),rnew]);
    phx=permute(phx,[2,1,3]);
    phi{i+1}=phx;
    %phi{i+1}=permute(phi{i+1},[1,3,2]);
end

ry(d+1)=rnew; %This value should not be modified :(((
%Truncate the core
cry=cry(1:pos1-1);
y.r=ry;
y.core=cry;
y.ps=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
 %Test back transform
%  y0=y;
%  y0.n=[2*ones(d,1);k];
%  y0.r=[y.r;1];
%  y0.d=d;
%  e0=eye(ry(d));
%  cry=[cry,e0(:)'];
%  y0.core=cry;
%  ww1=full(y0); ww1=reshape(ww1,[numel(ww1)/k,k]);
%  bm=ww0'*ww1;
%  norm(ww1-ww0*bm)
%keyboard;
phi{d+1}=1; %Bydlocode, but seems necessary
y.r=ry;
y.core=cry;
y.ps=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
psy=y.ps;
%Now start the main DMRG sweep
swp=1;
not_converged=true;
err_max = 0;
%ry(1)=1;
ry(d+1)=1;
dir='rl';
i=d-1;
cry_left=cry(1:psy(d)-1);
cry_right=cry(psy(d):psy(d+1)-1);
while ( swp <= nswp && not_converged )
       %As usual, compute (local) B-matrix in the TT-format
       cra1=cra(psa(i):psa(i+1)-1);
       cra2=cra(psa(i+1):psa(i+2)-1);
       px1=phi{i}; px1=reshape(px1,[ra(i),ry(i),ry(i)]);
       %px1=permute(px1,[1,2,3]);
       px1=permute(px1,[1,3,2]);
       px1=reshape(px1,[ra(i),ry(i)*ry(i)]);
       px2=phi{i+2};
       px2=reshape(px2,[ra(i+2),ry(i+2),ry(i+2)]);
       %px2=permute(px2,[1,3,2]);
       %Compute the local matrix just by putting px1 into cra1, px2 into
       %cra2
       cra2=reshape(cra2,[ra(i+1)*n(i+1)*n(i+1),ra(i+2)]);
       px2=reshape(px2,[ra(i+2),ry(i+2)*ry(i+2)]);
       b2=cra2*px2; %
       b1=px1.'*reshape(cra1,[ra(i),numel(cra1)/ra(i)]);
       b1=reshape(b1,[ry(i),ry(i),n(i),n(i),ra(i+1)]); b1_save=b1; %Save for phi computations
       b1=permute(b1,[1,3,2,4,5]);
       b1=reshape(b1,[ry(i)*n(i),ry(i)*n(i),ra(i+1)]);
       b2=reshape(b2,[ra(i+1),n(i+1),n(i+1),ry(i+2),ry(i+2)]);
       b2_save=b2; %Save for phi computations
       b2=permute(b2,[2,4,3,5,1]);
       b2=reshape(b2,[n(i+1)*ry(i+2),n(i+1)*ry(i+2),ra(i+1)]);
       mm=cell(2,1); mm{1}=b1; mm{2}=b2;
       cry=[cry_left(:);cry_right(:)];
       y.r=ry;
       y.core=cry;
       y.ps=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
       %mm1=tt_matrix(mm);
       %Now setup the initial guess: i the core
       if ( strcmp(dir,'rl') ) %The block index is in the second core
         pos=numel(cry_left);
         cry1=cry_left(pos-ry(i)*n(i)*ry(i+1)+1:pos);
         cry2=cry_right(1:ry(i+1)*n(i+1)*k*ry(i+2));
         cry1=reshape(cry1,[ry(i)*n(i),ry(i+1)]);
         cry2=reshape(cry2,[ry(i+1),n(i+1)*k*ry(i+2)]);
         w=cry1*cry2; w=reshape(w,[ry(i),n(i),n(i+1),k,ry(i+2)]);
         w=permute(w,[1,2,3,5,4]); w=reshape(w,[numel(w)/k,k]);
       else %The block index is in the first core
           pos=numel(cry_left);
           cry1=cry_left(pos-ry(i)*n(i)*k*ry(i+1)+1:pos);
           cry2=cry_right(1:ry(i+1)*n(i+1)*ry(i+2));
           cry1=reshape(cry1,[ry(i)*n(i)*k,ry(i+1)]);
           cry2=reshape(cry2,[ry(i+1),n(i+1)*ry(i+2)]);
           w=cry1*cry2; w=reshape(w,[ry(i),n(i),k,n(i+1),ry(i+2)]);
           w=permute(w,[1,2,4,5,3]); w=reshape(w,[numel(w)/k,k]);
       end
       %Now run the eigenvalue solver
        bw=bfun(mm,w); ev=bw'*w;
           %er0=norm(bw-w*ev,'fro')/norm(w,'fro');
           er_old=bw-w*ev;
        erc=zeros(1,size(w,2));
        for j=1:size(w,2)
            erc(j)=norm(er_old(:,j));
        end
           er0=norm(er_old,'fro')/norm(w,'fro');
       if ( size(w,1) >= max(5*k,msize) )
           matvec='bfun';
           [wnew,ev,fail_flag]=lobpcg(w,@(x) bfun(mm,x),local_eps,max_l_steps);
       else
          matvec='full';
          fm=full(tt_matrix(mm));
          [v,dg]=eig(fm);
          %[v,dg]=eigs(sparse(fm),k+1);
          %keyboard;
          ev=diag(dg);
          [ev,ind]=sort(ev,'ascend');
          v=v(:,ind);
          wnew=v(:,1:k);
          ev=ev(1:k);
       end
       er1=norm(bfun(mm,wnew)-wnew*diag(ev),'fro')/norm(wnew,'fro');

       % Check the distance between iterations
       dx = norm(wnew*(wnew'*w)-w);

       fv=sum(ev); %The functional we minimize;
       if ( verb>1 )
         fprintf('sweep=%d block=%d fv=%10.15f loc solve=%3.2e old_solve=%3.2e \n',swp,i,fv,er1,er0);
         disp(erc);
       end
       if ( strcmp(dir,'rl') ) %Implant the auxiliary core into the i-th core
           %(a1,i1,a2,a2,i2*g,a3)-> (a1,i1*g,a2,a2,i2,a3)
           %Delete old block from the core_left, add new block to the core
           %right

           %Prepare the truncation block
           rhs=wnew*diag(ev);
            if (strcmp(matvec,'full'))
                res_true = norm(fm*wnew-rhs)/norm(rhs);
            else
                res_true = norm(bfun(mm,wnew)-rhs)/norm(rhs);
            end;

           wnew=reshape(wnew,[ry(i),n(i),n(i+1),ry(i+2),k]);
           wnew=permute(wnew,[1,2,5,3,4]); wnew=reshape(wnew,[ry(i)*n(i)*k,n(i+1)*ry(i+2)]);
           [u,s,v]=svd(wnew,'econ'); s=diag(s);
           %Truncation block
           rnew=my_chop2(s,local_eps*norm(s));

           %%%
           rnew = min(rnew, numel(s));
           rnew = min(rnew, rmax);
           %%%%

           u=u(:,1:rnew); s=s(1:rnew); v=v(:,1:rnew);% v=v';
           u=u*diag(s); %u has to be reshaped

           % Residual truncation
%            r0=1; r1=min(numel(s),rmax);
%            r=1;
%            while ( (r ~= r0 || r ~= r1) && r0 <= r1)
%             r=min(floor((r0+r1)/2),rmax);
%             %er0=norm(s(r+1:numel(s)));
%             u1=u(:,1:r)*diag(s(1:r));
%             %Sonate u1
%             u1=reshape(u1,[ry(i)*n(i),k,r]);
%             u1=permute(u1,[1,3,2]);
%             u1=reshape(u1,[numel(u1)/k,k]);
%             [u2,~,v2]=svd(u1,'econ');
%             u1=u2*v2';
%             u1=reshape(u1,[ry(i)*n(i),r,k]);
%             u1=permute(u1,[1,3,2]);
%             u1=reshape(u1,[numel(u1)/r,r]);
%             sol = u1*(v(:,1:r))';
%
%             sol = reshape(sol,[ry(i),n(i),k,n(i+1),ry(i+2)]);
%             sol=permute(sol,[1,2,4,5,3]); sol=reshape(sol,[numel(sol)/k,k]);
%             %if ( norm(sol'*sol-eye(k))>1e-3 )
%             %  keyboard
%             %end
%             if (strcmp(matvec,'full'))
%                 resid = norm(fm*sol-rhs)/norm(rhs);
%             else
%                 resid = norm(bfun(mm,sol)-rhs)/norm(rhs);
%             end;
%             if ( verb )
%             fprintf('sweep %d, block %d, r0=%d, r1=%d, r=%d, resid=%g, MatVec=%s\n', swp, i, r0, r1, r, resid,matvec);
%             end
%             if ((resid<max(res_true*1.2, eps)) ) %Value of the rank is OK
%               r1=r;
%             else %Is not OK.
%               r0=min(r+1,rmax);
%             end;
%
%            end
%            rnew=r;
%            u=u1; v=v(:,1:rnew);

%            if ( norm(sol'*sol-eye(k)) > 1e-7 )
%              keyboard;
%            end
           %u=u(:,1:rnew); s=s(1:rnew); v=v(:,1:rnew);% v=v';
           %u=u*diag(s); %u has to be reshaped

           %Random restart block
           radd=min(kickrank,size(v,1)-rnew);
           rnew=rnew+radd;
           if ( radd >  0 )
             vr=randn(size(v,1),radd);
             ur=zeros(size(u,1),radd);
             %Orthogonalize vr to v by Golub-Kahan reorth
             mvr=v'*vr; vnew=vr-v*mvr;
             reort_flag=false;
             for j=1:radd
                if ( norm(vnew(:,j)) <= 0.5*norm(vr(:,j)))
                   reort_flag=true;
                end
             end
             if (reort_flag)
                 sv=v'*vnew;
                 %mvr=mvr+v'*vnew;
                 vnew=vnew-v*sv;
             end
             [vnew,~]=qr(vnew,0);

             v=[v,vnew];
             u=[u,ur];
             %norm(u*v'-u1*v1');
             %keyboard;
             %keyboard;
           end
           v=v';

           %Memory stuff
           pos=numel(cry_left);
           cry_left(pos-ry(i)*n(i)*ry(i+1)+1:pos)=[];
           cry_right(1:ry(i+1)*n(i+1)*k*ry(i+2))=[]; %Delete the top core from cry_right
           cry_right=[u(:)', v(:)',cry_right]; %Simply add new block to cry_right
           ry(i+1)=rnew;

           %Recalculate phi block; we need to recalculate phi{i+1} here
           %using phi{i+2}
           %px2(ra(i+2),ry(i+2),ry(i+2))*cra2(ra(i+1),n(i),m(i),ra(i+2)*v(ry(i+1),n(i+1),ry(i+2))*v(ry(i+1),n(i+1),ry(i+2))
           %we already have b2 ---  (b2_save)
           %(ra(i+1),n(i+1),n(i+1),ry(i+2),ry(i+2)), convolve over the
           %n(i+1),ry(i+2)
           phx=reshape(b2_save,[ra(i+1),n(i+1),n(i+1),ry(i+2),ry(i+2)]);
           phx=permute(phx,[2,4,1,3,5]);
           %phx=permute(phx,[3,4,1,2,5]);
           %phx=permute(phx,[3,5,1,2,4]);
           % phx=permute(phx,[2,5,1,3,4]);
           phx=reshape(phx,[n(i+1)*ry(i+2),ra(i+1)*n(i+1)*ry(i+2)]);
           v0=v;
           v0=reshape(v0,[ry(i+1),n(i+1)*ry(i+2)]);
           phx=v0*phx;
           phx=reshape(phx,[ry(i+1),ra(i+1),n(i+1),ry(i+2)]);
           phx=permute(phx,[3,4,1,2]);
           phx=reshape(phx,[n(i+1)*ry(i+2),ry(i+1)*ra(i+1)]);
           phx=v0*phx;
           phx=reshape(phx,[ry(i+1),ry(i+1),ra(i+1)]);
           %phx=permute(phx,[3,2,1]);
           phx=permute(phx,[3,2,1]);
           phi{i+1}=phx;
       else %Implant the auxiliary core from the left block to the right block

           %Prepare the truncation block
           rhs=wnew*diag(ev);
            if (strcmp(matvec,'full'))
                res_true = norm(fm*wnew-rhs)/norm(rhs);
            else
                res_true = norm(bfun(mm,wnew)-rhs)/norm(rhs);
            end;
           wnew=reshape(wnew,[ry(i),n(i),n(i+1),ry(i+2),k]);
           wnew=permute(wnew,[1,2,3,5,4]); wnew=reshape(wnew,[ry(i)*n(i),n(i+1)*k*ry(i+2)]);
           [u,s,v]=svd(wnew,'econ'); s=diag(s);
           rnew=my_chop2(s,local_eps*norm(s));

           %%%%
           rnew = min(rnew, rmax);
           rnew = min(rnew, numel(s));
           %%%%

           u=u(:,1:rnew); s=s(1:rnew); v=v(:,1:rnew);% v=v';
           v=v*diag(s); %u has to be reshaped

           % Residual truncation
%            r0=1; r1=min(size(s,1),rmax);
%            r=1;
%            while ( (r ~= r0 || r ~= r1) && r0 <= r1)
%             r=min(floor((r0+r1)/2),rmax);
%             %er0=norm(s(r+1:numel(s)));
%             v1=v(:,1:r)*diag(s(1:r));
%             %Sonate v1
%             v1=reshape(v1,[n(i+1),k,ry(i+2),r]);
%             v1=permute(v1,[1,3,4,2]);
%             v1=reshape(v1,[numel(v1)/k,k]);
%             [u2,~,v2]=svd(v1,'econ');
%             v1=u2*v2';
%             v1=reshape(v1,[n(i+1),ry(i+2),r,k]);
%             v1=permute(v1,[1,4,2,3]);
%             v1=reshape(v1,[numel(v1)/r,r]);
%             sol=u(:,1:r)*v1';
%             sol = reshape(sol,[ry(i),n(i),n(i+1),k,ry(i+2)]);
%             sol=permute(sol,[1,2,3,5,4]); sol=reshape(sol,[numel(sol)/k,k]);
%             if (strcmp(matvec,'full'))
%                 resid = norm(fm*sol-rhs)/norm(rhs);
%             else
%                 resid = norm(bfun(mm,sol)-rhs)/norm(rhs);
%             end;
%             if ( verb )
%             fprintf('sweep %d, block %d, r0=%d r1=%d r=%d, resid=%g, MatVec=%s\n', swp, i, r0, r1, r, resid,matvec);
%             end
%             if ((resid<max(res_true*1.2, eps)) ) %Value of the rank is OK
%               r1=r;
%             else %Is not OK.
%               r0=min(r+1,rmax);
%             end;
%            end
%            rnew=r;
%            %Truncation block
%            %rnew=my_chop2(s,eps*norm(s));
%            u=u(:,1:rnew); v=v1;

           %Random restart block
           radd=min(kickrank,size(u,1)-rnew);
           rnew=rnew+radd;
           if ( radd >  0 )
             vr=zeros(size(v,1),radd);
             ur=randn(size(u,1),radd);
             %Orthogonalize vr to v by Golub-Kahan reorth
             mur=u'*ur; unew=ur-u*mur;
             reort_flag=false;
             for j=1:radd
                if ( norm(unew(:,j)) <= 0.5*norm(ur(:,j)))
                   reort_flag=true;
                end
             end
             if (reort_flag)
                 sv=u'*unew;
                 %mvr=mvr+v'*vnew;
                 unew=unew-u*sv;
             end
             [unew,~]=qr(unew,0);

             u=[u,unew];
             v=[v,vr];
           end
           v=v';

           cry_right(1:ry(i+1)*n(i+1)*ry(i+2))=[];
           pos=numel(cry_left);
           cry_left(pos-ry(i)*n(i)*k*ry(i+1)+1:pos)=[];
           cry_left=[cry_left,u(:)',v(:)'];

           ry(i+1)=rnew;

           %Recalculate phi block; we need to recalculate phi{i+1} using
           %phi{i}
           phx=reshape(b1_save,[ry(i),ry(i),n(i),n(i),ra(i+1)]);
           u0=u; u0=reshape(u0,[ry(i)*n(i),ry(i+1)]);
           phx=permute(phx,[1,3,5,2,4]);
           %phx=permute(phx,[2,4,5,1,3]);
           %phx=permute(phx,[1,4,5,2,3]);
           %phx=permute(phx,[2,3,5,1,4]);
           phx=reshape(phx,[ry(i)*n(i)*ra(i+1),ry(i)*n(i)]);
           phx=phx*u0;
           phx=reshape(phx,[ry(i),n(i),ra(i+1),ry(i+1)]);
           phx=permute(phx,[3,4,1,2]);
           phx=reshape(phx,[ra(i+1)*ry(i+1),ry(i)*n(i)]);
           phx=phx*u0; phx=reshape(phx,[ra(i+1),ry(i+1),ry(i+1)]);
           phx=permute(phx,[1,2,3]);
           phi{i+1}=phx;
       end

   err_max = max(err_max, dx);
   %Choose the next direction block; now implement the simple case
   if ( strcmp(dir,'rl') )
     if ( i > 1 )
       i=i-1;
     else %Change direction %The last optimization was for (1,2) core
       dir='lr';
       %One block should go from cry_right to cry_left
       cry_left=cry_right(1:ry(1)*n(1)*ry(2)*k); %This seems correct
       cry_right(1:ry(1)*n(1)*ry(2)*k)=[];
       if (verb>0)
        fprintf('--- sweep=%d, err_max=%3.3e \n',swp, err_max);
       end;
       swp=swp+1;
       not_converged = (err_max>eps);
       err_max = 0;
     end
   else
    if ( i < d-1 )
        i=i+1;
     else
       dir='rl';
       pos=numel(cry_left);
       cry_right=cry_left(pos-ry(d)*n(d)*ry(d+1)*k+1:pos);
       cry_left(pos-ry(d)*n(d)*ry(d+1)*k+1:pos)=[];
       if (verb>0)
        fprintf('--- sweep=%d, err_max=%3.3e \n',swp, err_max);
       end;
       swp=swp+1;
       not_converged = (err_max>eps);
       err_max = 0;
       %One block should go from cry_left to cry_right (?) --- seems no :)
     end
   end

end
  %Gather the solution

   ry(d+1)=k; %This is the final
   y.r=ry;
   cry=[cry_left,cry_right];
   y.core=cry;
   y.r=ry;
   y.d=d;
   y.ps=cumsum([1;n.*ry(1:d).*ry(2:d+1)]);
end

    function [x]=bfun(a,x)
    %[Y]=BFUN(A,X,K)
    %a is given as U(i1,j1,s)*V(i2,j2,s)
    %\sum_{j1,j2,s} U(i1,j1,s)*V(i2,j2,s)*X(j1,j2,q)
    re=size(x,2);
    ul=a{1}; vl=a{2};
    n1=size(ul,1);m1=size(ul,2); ral=size(ul,3);
    n2=size(vl,1); m2=size(vl,2);
    ul=permute(ul,[1,3,2]); ul=reshape(ul,[numel(ul)/m1,m1]);
    x=reshape(x,[m1,numel(x)/m1]);
    x=ul*x; %x is i1xsxj2xq s,j2
    x=reshape(x,[n1,ral,m2,re]);
    x=permute(x,[3,2,1,4]);
    x=reshape(x,[m2*ral,n1*re]);
    vl=reshape(vl,[n2,m2*ral]);
    x=vl*x; %is n2*n1*k
    x=reshape(x,[n2,n1,re]);
    x=permute(x,[2,1,3]);
    x=reshape(x,[numel(x)/re,re]);
    return
    end


function [optimState,t_func] = initdesign_vbmc(optimState,Ns,funwrapper,t_func,options)
%INITDESIGN_VBMC Initial sample design (provided or random box).

x0 = optimState.Cache.X_orig;
[N0,D] = size(x0);

if N0 <= Ns
    Xs = x0;
    ys = optimState.Cache.y_orig;
    if N0 < Ns
        switch lower(options.InitDesign)
            case 'plausible'
                % Uniform random samples in the plausible box (in transformed space)
                Xrnd = bsxfun(@plus,bsxfun(@times,rand(Ns-N0,D),optimState.PUB-optimState.PLB),optimState.PLB);
            case 'narrow'
                xstart = warpvars_vbmc(x0(1,:),'dir',optimState.trinfo);
                Xrnd = bsxfun(@plus,bsxfun(@times,rand(Ns-N0,D)-0.5,0.1*(optimState.PUB-optimState.PLB)),xstart);
                Xrnd = bsxfun(@min,bsxfun(@max,Xrnd,optimState.PLB),optimState.PUB);
            otherwise
                error('Unknown initial design for VBMC.');
        end
        Xrnd = warpvars_vbmc(Xrnd,'inv',optimState.trinfo);  % Convert back to original space
        Xs = [Xs; Xrnd];
        ys = [ys; NaN(Ns-N0,1)];
    end
    idx_remove = true(N0,1);

elseif N0 > Ns
    % Cluster starting points
    kmeans_options = struct('Display','off','Method',2,'Preprocessing','whiten');
    idx = fastkmeans(x0,Ns,kmeans_options);

    % From each cluster, take points with higher density in original space
    Xs = NaN(Ns,D); ys = NaN(Ns,1); idx_remove = false(N0,1);
    for iK = 1:Ns
        idxK = find(idx == iK);
        xx = optimState.Cache.X_orig(idxK,:);
        yy = optimState.Cache.y_orig(idxK);
        [~,idx_y] = max(yy);
        Xs(iK,:) = xx(idx_y,:);
        ys(iK) = yy(idx_y);
        idx_remove(idxK(idx_y)) = true;
    end
end
% Remove points from starting cache
optimState.Cache.X_orig(idx_remove,:) = [];
optimState.Cache.y_orig(idx_remove) = [];

Xs = warpvars_vbmc(Xs,'d',optimState.trinfo);

for is = 1:Ns
    timer_func = tic;
    if isnan(ys(is))    % Function value is not available
        [~,optimState] = funlogger_vbmc(funwrapper,Xs(is,:),optimState,'iter');
    else
        [~,optimState] = funlogger_vbmc(funwrapper,Xs(is,:),optimState,'add',ys(is));
    end
    t_func = t_func + toc(timer_func);
end

end


function [output_stack, IRR, ORR, ARR] = evalfisx(input_stack, fis)
%EVALFISX Evaluation of a fuzzy inference system.
%
%   See evalfis1 for syntax and explanation.
%
%   It is completely based on MATLAB's evalfis1
%   with some modifications, so it's compatible
%   with extendent fuzzy rule structure.
%
%   Compare also code of this function
%   with code of original the evalfis1.
%
%   Example:
%   load carsmall;
%   fis = genfis4([Weight, Displacement], Acceleration, 'mamdani');
%   out = evalfisx([2000 100; 2000 200; 2000 300], fis);

%   Per Konstantin A. Sidelnikov, 2009.

persistent CURRENT_FIS;
persistent FIS_NAME FIS_TYPE IN_N OUT_N IN_MF_N OUT_MF_N RULE_N;
persistent AND_METHOD OR_METHOD IMP_METHOD AGG_METHOD DEFUZZ_METHOD;
persistent BOUND IN_MF_TYPE OUT_MF_TYPE;
persistent IN_RULE_LIST OUT_RULE_LIST AND_OR RULE_WEIGHT;
persistent IN_PARAM OUT_PARAM;
persistent OUT_TEMPLATE_MF OUT_MF QUALIFIED_OUT_MF OVERALL_OUT_MF;

point_n = 101;
mf_para_n = 4;

initialization = 1;
% Check if initialization necessary.
if isequal(fis, CURRENT_FIS)
    initialization = 0;
end

% Unpack data and initialize global variables.
if initialization
    CURRENT_FIS = fis;
    FIS_NAME = fis.name;
    FIS_TYPE = fis.type;
    IN_N = length(fis.input);
    OUT_N = length(fis.output);

    for i = 1 : IN_N
        IN_MF_N(i) = length(fis.input(i).mf);
    end
    for i = 1 : OUT_N
        OUT_MF_N(i) = length(fis.output(i).mf);
    end

    RULE_N = length(fis.rule);

    AND_METHOD = fis.andMethod;
    OR_METHOD =  fis.orMethod;
    IMP_METHOD = fis.impMethod;
    AGG_METHOD = fis.aggMethod;
    DEFUZZ_METHOD = fis.defuzzMethod;

    IN_MF_TYPE = [];
    for i = 1 : IN_N
        IN_MF_TYPE = [IN_MF_TYPE; {fis.input(i).mf.type}'];
    end
    OUT_MF_TYPE = [];
    for i = 1 : OUT_N
        OUT_MF_TYPE = [OUT_MF_TYPE; {fis.output(i).mf.type}'];
    end

    in_bound = reshape([fis.input.range], IN_N, 2);
    out_bound = reshape([fis.output.range], OUT_N, 2);
    BOUND = [in_bound; out_bound];

    RULE_WEIGHT = [fis.rule.weight]';
    AND_OR = [fis.rule.connection]';
    IN_RULE_LIST = {fis.rule.antecedent}';
    OUT_RULE_LIST = reshape([fis.rule.consequent], RULE_N, OUT_N);

    k = 1;
    totalInputMFs = sum(IN_MF_N);
    totalOutputMFs = sum(OUT_MF_N);
    IN_PARAM = zeros(totalInputMFs, 4);
    for i = 1 : IN_N
        for j = 1 : length(fis.input(i).mf)
            tmp = fis.input(i).mf(j).params;
            IN_PARAM(k, 1 : length(tmp)) = tmp;
            k = k + 1;
        end
    end
    k = 1;
    OUT_PARAM = zeros(totalOutputMFs, 4);
    for i = 1 : OUT_N
        for j = 1 : length(fis.output(i).mf)
            tmp = fis.output(i).mf(j).params;
            OUT_PARAM(k, 1 : length(tmp)) = tmp;
            k = k + 1;
        end
    end

    if strcmp(FIS_TYPE, 'sugeno')
        OUT_PARAM = OUT_PARAM(:, 1 : IN_N + 1);
    elseif strcmp(FIS_TYPE, 'mamdani')
        OUT_PARAM = OUT_PARAM(:, 1 : mf_para_n);
    else
        error('Unknown FIS type!');
    end

    if strcmp(FIS_TYPE, 'mamdani')
        % Compute OUT_TEMPLATE_MF
        OUT_TEMPLATE_MF = zeros(sum(OUT_MF_N), point_n);
        cum_mf = cumsum(OUT_MF_N);
        for i = 1 : sum(OUT_MF_N)
            % index for output
            output_index = find((cum_mf - i) >= 0, 1, 'first');
            tmp = linspace(BOUND(IN_N + output_index, 1), ...
                BOUND(IN_N + output_index, 2), point_n);
            OUT_TEMPLATE_MF(i, :) = ...
                evalmf(tmp, OUT_PARAM(i, :), OUT_MF_TYPE{i});
        end

        % Reorder to fill OUT_MF, an (RULE_N X point_n * OUT_N) matrix.
        OUT_MF = zeros(RULE_N, point_n * OUT_N);
        for i = 1 : RULE_N
            for j = 1 : OUT_N
                mf_index = OUT_RULE_LIST(i, j);
                index = sum(OUT_MF_N(1 : j - 1)) + abs(mf_index);
                tmp = (j - 1) * point_n + 1 : j * point_n;
                if mf_index > 0    % regular MF
                    OUT_MF(i, tmp) = OUT_TEMPLATE_MF(index, :);
                elseif mf_index < 0 % Linguistic hedge "NOT"
                    OUT_MF(i, tmp) = 1 - OUT_TEMPLATE_MF(index, :);
                else                % Don't care (MF index == 0)
                    OUT_MF(i, tmp) = ones(1, point_n);
                end
            end
        end

        % Allocate other matrices
        QUALIFIED_OUT_MF = zeros(RULE_N, point_n * OUT_N);
        OVERALL_OUT_MF = zeros(1, point_n * OUT_N);
    end
    %   fprintf('Global variables for %s FIS are initialized\n', FIS_NAME);
end
% End of initialization

% Error checking for input stack
m = size(input_stack, 1);
n = size(input_stack, 2);
if ~((n == IN_N) || ((n == 1) && (m == IN_N)))
    fprintf('The input stack is of size %dx%d,', m, n);
    fprintf('while expected input vector size is %d.\n', IN_N);
    error('Exiting ...');
end
if (n == 1) && (m == IN_N)
    data_n = 1;
    input_stack = input_stack';
else
    data_n = m;
end

% Allocate output stack
output_stack = zeros(data_n, OUT_N);

% Iteration through each row of input stack
for kkk = 1 : data_n
    input = input_stack(kkk, :);

    % Find in_template_mf_value
    in_template_mf_value = zeros(sum(IN_MF_N), 1);
    cum_mf = cumsum(IN_MF_N);
    for i = 1 : sum(IN_MF_N)
        input_index = find((cum_mf - i) >= 0, 1, 'first');
        in_template_mf_value(i) = ...
            evalmf(input(input_index), IN_PARAM(i, :), IN_MF_TYPE{i});
    end

    % Reordering to fill in_mf_value, which is an (RULE_N X 1) cell matrix.
    tmp = cumsum([0, IN_MF_N(1 : IN_N - 1)]);
    in_mf_value = cell(RULE_N, 1);
    for ind = 1 : RULE_N
        index = tmp(IN_RULE_LIST{ind}(1, :)) + abs(IN_RULE_LIST{ind}(2, :));
        in_mf_value{ind} = in_template_mf_value(index)';
        % Take care of linguistic hedge NOT (MF index is negative)
        neg_index = find(IN_RULE_LIST{ind}(2, :) < 0);
        in_mf_value{ind}(neg_index) = 1 - in_mf_value{ind}(neg_index);
    end

    % Find the firing strengths
    % AND_METHOD = 'min' or 'prod'; which is used as function name too
    % OR_METHOD = 'max' or 'probor'; which is used as function name too
    firing_strength = zeros(RULE_N, 1);
    and_index = find(AND_OR == 1);
    or_index = find(AND_OR == 2);
    if IN_N ~= 1
        firing_strength(and_index) = ...
            cellfun(@(x) feval(AND_METHOD, x'), in_mf_value(and_index, :))';
        firing_strength(or_index) = ...
            cellfun(@(x) feval(OR_METHOD, x'), in_mf_value(or_index, :))';
    else
        firing_strength = [in_mf_value{:}]';
    end

    % Recalculate firing strengths scaled by rule weights
    firing_strength = firing_strength .* RULE_WEIGHT;

    % Find output
    if strcmp(FIS_TYPE, 'sugeno')
        template_output = OUT_PARAM * [input(:); 1]; % Output for template

        % Reordering according to the output part of RULE_LIST;
        % Negative MF index will becomes positive
        tmp = cumsum([0, OUT_MF_N(1 : OUT_N - 1)]);
        index = repmat(tmp, RULE_N, 1) + abs(OUT_RULE_LIST);
        index(index == 0) = 1;   % temp. setting for easy indexing
        rule_output = template_output(index);
        rule_output(OUT_RULE_LIST == 0) = 0;  % take care of zero index
        sum_firing_strength = sum(firing_strength);

        if sum_firing_strength == 0
            fprintf('input = [');
            for i = 1 : IN_N
                fprintf('%f ', input(i));
            end
            fprintf(']\n');
            error('Total firing strength is zero!');
        end

        switch DEFUZZ_METHOD
            case 'wtaver'
                output_stack(kkk, :) = firing_strength' * rule_output / ...
                    sum_firing_strength;
            case 'wtsum'
                output_stack(kkk, :) = firing_strength' * rule_output;
            otherwise
                error('Unknown defuzzification method!');
        end
    elseif strcmp(FIS_TYPE, 'mamdani')
        % Transform OUT_MF to QUALIFIED_OUT_MF
        % Duplicate firing_strength.
        tmp = firing_strength(:, ones(1, point_n * OUT_N));

        if strcmp(IMP_METHOD, 'prod')      % IMP_METHOD == 'prod'
            QUALIFIED_OUT_MF = tmp .* OUT_MF;
        elseif strcmp(IMP_METHOD, 'min')   % IMP_METHOD == 'min'
            QUALIFIED_OUT_MF = feval(IMP_METHOD, tmp, OUT_MF);
        else    % IMP_METHOD is user-defined
            tmp1 = feval(IMP_METHOD, [tmp(:)'; OUT_MF(:)']);
            QUALIFIED_OUT_MF = reshape(tmp1, RULE_N, point_n * OUT_N);
        end

        % AGG_METHOD = 'sum' or 'max' or 'probor' or user-defined
        OVERALL_OUT_MF = feval(AGG_METHOD, QUALIFIED_OUT_MF);

        for i = 1 : OUT_N
            tmp = linspace(BOUND(IN_N + i, 1), BOUND(IN_N + i, 2), point_n);
            tmp1 = (i - 1) * point_n + 1 : i * point_n;
            output_stack(kkk, i) = ...
                defuzz(tmp, OVERALL_OUT_MF(1, tmp1), DEFUZZ_METHOD);
        end
    else
        fprintf('fis_type = %d\n', FIS_TYPE);
        error('Unknown FIS type!');
    end
end

if nargout >= 2
    IRR = in_mf_value;
end

if strcmp(FIS_TYPE, 'sugeno')
    if nargout >= 3
        ORR = rule_output;
    end
    if nargout >= 4
        ARR = firing_strength(:, ones(1, OUT_N));
    end
else
    if nargout >= 3
        ORR = [];
        for iii = 1 : OUT_N
            tmp = (iii - 1) * point_n + 1 : iii * point_n;
            ORR = [ORR; QUALIFIED_OUT_MF(:, tmp)];
        end
        ORR = ORR';
    end
    if nargout >= 4
        ARR = reshape(OVERALL_OUT_MF, point_n, OUT_N);
    end
end


function relaxMtFit(dataDir,outDir,voxRange)
%
% relaxMtFit(dataDir,outDir,voxRange)
%
% Calculates the k and f maps using the MT TOF sequence.
%
% See Yarnykh & Yuan (2004). Cross-relaxation imaging reveals
% detailed anatomy of white matter fiber tracts in the human
% brain. Neuroimage, 23(1):409-24. (PMID: 15325389)
%
% KEY DEPENDENCIES:
% lorentzian.m, relaxMtFitFunc.m
%
% SEE ALSO:
% relaxPreprocess.m to get raw DICOM data into the right format for
% this function.
%
% HISTORY:
% 2006.02.19 Nikola Stikov wrote it.
% 2007.01.?? NS recoded it to use lsqnonlin
% 2007.02.27 RFD rewrote the loop structure and data format.
% 2007.03.02 RFD slight performance enhancements. Also renamed
% several functions and added them all to the mrDiffusion
% repository.
%

if(~exist('dataDir','var')|isempty(dataDir))
  dataDir = pwd;
end

if(~exist('outDir','var')|isempty(outDir))
  outDir = dataDir;
end

if(~exist('voxRange','var')|isempty(voxRange))
  voxRange = '';
end

if(~exist(outDir,'dir'))
  mkdir(outDir);
end

disp(['Loading data from ' dataDir '...']);

ni = niftiRead(fullfile(dataDir,'T1.nii.gz'));
T1 = ni.data;
xform = ni.qto_xyz;
clear ni;
nz = T1>0;

if(exist(fullfile(dataDir,'brainMask.nii.gz'),'file'))
  ni = niftiRead(fullfile(dataDir,'brainMask.nii.gz'));
  brainMask = ni.data==1;
  clear ni;
else
  brainMask = nz;
end

%ni = niftiRead(fullfile(dataDir,'PD.nii.gz'));
%PD = ni.data;

ni = niftiRead(fullfile(dataDir,'S0.nii.gz'));
S0 = ni.data;
clear ni;

d = dir(fullfile(dataDir,'MT_*.nii.gz'));
delta = zeros(1,length(d));
for(ii=1:length(d))
  ni = niftiRead(fullfile(dataDir,d(ii).name));
  MT(:,:,:,ii) = ni.data;
  clear ni;
  % TODO: fix this crude hack. Maybe store offset freqs in nifti header?
  delta(ii) = sscanf(d(ii).name,'MT_%dkHz.nii.gz') ;
end
delta = delta'*1e3; % offset frequencies, in Hz

iterMax = 50; %maximum number of iterations allowed

sz = size(brainMask);

% Clip T1 and PD to reasonable values
% Tissue       T1 (s)          T2 (ms)        PD*
% CSF        0.8 - 20        110 - 2000    70 - 230
% White	    0.76 - 1.08       61 - 100 	   70 - 90
% Gray      1.09 - 2.15       61 - 109     85 - 125
% Meninges   0.5 - 2.2        50 - 165      5 - 44
% Muscle    0.95 - 1.82       20 - 67      45 - 90
% Adipose    0.2 - 0.75       53 - 94      50 - 100
% (PD values are based on PD=111 for 12mM aqueous NiCl2)
% (From http://www.cis.rit.edu/htbooks/mri/chap-8/chap-8.htm#8.7 )
% Our PDs seem to be scaled by 50 or so???
T1(T1<0.01) = 0.01;
T1(T1>10) = 10;
%PD(PD<0) = 0;
%PD(PD>12000) = 12000;

R1 = zeros(size(T1)); R1(nz) = 1./T1(nz);
%if(~exist(fullfile(dataDir,'R1.nii.gz'),'file'))
%  dtiWriteNiftiWrapper(single(R1),xform,fullfile(dataDir,'R1.nii.gz'));
%end

% Tidy-up the brain mask a bit
%brainMask(PD<1000|PD>10000|T1<0.2|T1>2|S0<20) = 0;
brainMask(T1<0.2|T1>2|any(MT==0,4)) = 0;

% RFD: empirically, [1 .08] is the median for brain tissue
x0 = [1 .08];
%x0 = [2.4, .10]'; %this is our initial guess, bese [3.4, .15]'

lb = [.1 .03]; % [k f]
ub = [5 .28];

t_m = 8e-3; %bese 8e-3
t_s = 5e-3; %bese 5e-3
t_r = 19e-3; %bese 19e-3
T2_B = 11e-6;
% Where does this come from? Should we estimate it from the data?
w1rms = 2400; % omega-1 RMS

for ii = 1:length(delta)
    W_B(ii) = pi*(w1rms^2)*lorentzian (delta(ii), T2_B);
end;

options = optimset('LevenbergMarquardt','on', 'Display', 'off');

% FOR DEBUGGING:
%showMontage(brainMask);

brainInds = find(brainMask);
numVoxelsPerUpdate = 10000;
nVoxAll = length(brainInds);
for(ii=1:size(MT,4))
  tmpVol = MT(:,:,:,ii);
  tmpMT(ii,:) = tmpVol(brainInds);
end
clear tmpVol;
MT = tmpMT;
clear tmpMT;
R1 = R1(brainInds);
S0 = S0(brainInds);

f = zeros(1,nVoxAll);
k = zeros(1,nVoxAll);
gof = zeros(1,nVoxAll);
totalSecs = 0;
if(~isempty(voxRange))
  if(any(voxRange<0))
	doVox = [floor(nVoxAll*voxRange(1))+1:ceil(nVoxAll*voxRange(2))];
  else
	doVox = [max(1,voxRange(1)):min(nVoxAll,voxRange(2))];
  end
  fprintf('Processing voxels %d - %d...\n', doVox(1), doVox(end));
else
   doVox = [1:nVoxAll];
   fprintf('Processing all %d voxels...\n',nVoxAll);
end

% What we compute here is actually W_F./R1_F. R1_F is computed in
% the fit function, but we precompute the rest out here to save a
% few cpu cycles in the loop below.
W_F = (w1rms./(2*pi*delta)).^2/.055;

warning off;
nVox = doVox(end)-doVox(1);
tmp = zeros(size(brainMask));
tmpName = tempname
tic;
for(ii=doVox)
  if(mod(ii,numVoxelsPerUpdate)==0)
    prevSecs = toc;
    totalSecs = totalSecs+prevSecs;
    secsPerVox = totalSecs./(ii-doVox(1));
    estTime = secsPerVox*(doVox(end)-ii);
    if(estTime>5400) estTime=estTime./3600; estTimeUnits='hours';
    elseif(estTime>90) estTime=estTime./60; estTimeUnits='minutes';
    else estTimeUnits='seconds'; end
    fprintf('Processed %d of %d voxels- %0.1f %s remaining (%0.3f secs per vox)...\n',ii-doVox(1),nVox,estTime,estTimeUnits,secsPerVox);
	tmp(brainInds) = f;
    m = makeMontage(tmp,[1:5:size(tmp,3)]);
	m = uint8(round(m./max(f).*255));
	imwrite(m,['/home/bob/public_html/f.png']);
	save(tmpName,'k','f','gof','voxRange','xform');
    tic;
  end

  % Some voxels produce a "Input to EIG must not contain NaN
  % or Inf" error in lsqnonl in. Tweaking the bounds or
  % starting estimate can fix it sometimes, but they are
  % probably junk voxels anyway, so we'll catch and skip them.
  try
    %[x, resnorm, residual, exitflag, output] = lsqnonlin(@(x) relaxMtFitFunc(x, MT(:,ii), W_B, W_F, T2_B, R1(ii), S0(ii), t_m, t_s, t_r), x0, lb, ub, options); %bese j-12;
    [x, resnorm, exitflag] = fminsearch(@(x) relaxMtFitFuncLs(x, MT(:,ii), W_B, W_F, T2_B, R1(ii), S0(ii), t_m, t_s, t_r), x0, options);
    if(exitflag>0)
      k(ii) = x(1);
      f(ii) = x(2);
      gof(ii) = resnorm;
    else
      gof(ii) = NaN;
    end
  catch
    % Leave the fit values at zero.
  end
end
warning on;

fprintf('Finished processing %d slices (%d voxels) in %0.1f seconds.\n\n',sz(3),nVox,totalSecs);

if(isempty(voxRange))
   outName = '';
else
   outName = sprintf('%0.2f-%0.2f',voxRange(1),voxRange(2));
end
im=zeros(size(brainMask)); im(brainInds) = f; f = im;
im=zeros(size(brainMask)); im(brainInds) = k; k = im;
im=zeros(size(brainMask)); im(brainInds) = gof; gof = im;
try
   dtiWriteNiftiWrapper(single(f),xform,fullfile(outDir,[outName 'f.nii.gz']));
   dtiWriteNiftiWrapper(single(k),xform,fullfile(outDir,[outName 'k.nii.gz']));
   dtiWriteNiftiWrapper(single(gof),xform,fullfile(outDir,[outName 'gof.nii.gz']));
catch
   outName = tempname
   save(outName,'k','f','gof','voxRange','xform');
end
% FOR DEBUGGING:
keyboard;
return;


%kkhypot 'Output = sqrt[(Input 1)**2 + (Input 2 or Constant)**2]'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros khypot.pane file
%
% Parameters:
% InputFile: i1 'Input 1', required: 'First input data object'
% OutputFile: o 'Output', required: 'Resulting output data object'
% InputFile: i2 'Input 2', optional: 'Second input data object'
%
% Example: o = kkhypot({i1, i2}, {'i1','';'o','';'i2',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% khypot - Output = sqrt[(Input 1)**2 + (Input 2 or Constant)**2]
%
%  DESCRIPTION
% The "Hypotenuse" operator returns the square root of the result
% produced by adding the square of each point in \fBInput 1" and the square
% of either the corresponding point in \fBInput 2" or the \fBConstant\fP
% value, which ever is specified by the user.
%
% Executing "Hypotenuse" runs the program \fIkarith2\fP with the -hypot flag.
%
%  "Data Type - Single Input" 5
% .cI $DATAMANIP/repos/shared/man/sections/value_type_1input
%
%  "Data Type - Two Input Objects" 5
% .cI $DATAMANIP/repos/shared/man/sections/value_type_2input
%
%  "Map Data - Single Input" 5
% .cI $DATAMANIP/repos/shared/man/sections/map_1input
%
%  "Map Data - Two Input Objects" 5
% .cI $DATAMANIP/repos/shared/man/sections/map_2input
%
%  "Validity Mask" 5
% .cI $DATAMANIP/repos/shared/man/sections/mask_2input
%
%  "Input Objects of Different Sizes" 5
% .cI $DATAMANIP/repos/shared/man/sections/resize_2input
% The values used to pad the data when input files are not the same size are
% (0.0, 0.0).
%
%  "Explicit Location and Time Data - Single Input" 5
% .cI $DATAMANIP/repos/shared/man/sections/loc_and_time_1input
%
%  "Explicit Location and Time Data - Two Input Objects" 5
% .cI $DATAMANIP/repos/shared/man/sections/loc_and_time_2input
%
%  "Failure Modes - Single Input"
% .cI $DATAMANIP/repos/shared/man/sections/fail_1input
%
%  "Failure Modes - Two Input Objects"
% .cI $DATAMANIP/repos/shared/man/sections/fail_2input
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
% DATAMANIP::karith2, DATAMANIP::kcmplx2real, DATAMANIP::kmag
%
%  RESTRICTIONS
% Operations on complex data are not supported at this time.  If the
% input object is complex, only the real component of the data is
% processed, and the output object is real.  To calculate the hypotenuse,
% or magnitude of a complex number, use DATAMANIP::kcmplx2real.
%
%  REFERENCES
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kkhypot(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kkhypot(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i1', '__input';'o', '__output';'i2', '__input'};
maxval={0,0,1};
minval={0,0,1};
istoggle=[0,0,1];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','InputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'karith2"  -hypot'],Inputs,narglist);


function [L_l,R_l] = lindemannInh(L,R,fs,c_inh,dim)
%LINDEMANNINH Signal pre-processing for Lindemann's cross-correlation
%
%   [L_L,R_L] = IOSR.AUDITORY.LINDEMANNINH(L,R,FS) pre-processes left L and
%   right R signals for the cross-correlation function based on Lindemann's
%   precedence model [1,2]. A crucial parameter for Lindemann's model is
%   the "operating point", which controls the amount of inhibition. The
%   parameter is actually a function of the input related to its RMS level.
%   After half-wave rectifying and filtering the input signals L and R
%   (sampled at FS Hz) along the first non-singleton dimension, this
%   function applies a gain related to the inhibition parameter C_INH
%   (default is 0.3). The gain is identical for all rows, columns, etc.
%   Lastly, values outside of the interval [0,1] are not permitted in
%   Lindemann's model and hence these values are clipped.
%
%   [L_L,R_L] = IOSR.AUDITORY.LINDEMANNINH(L,R,FS,C_INH) uses the specified
%   inhibition parameter C_INH. The value must be in the interval [0,1].
%
%   [L_L,R_L] = IOSR.AUDITORY.LINDEMANNINH(L,R,FS,C_INH,DIM) pre-processes
%   L and R along the dimension DIM.
%
%   References
%
%   [1] Lindemann, W. (1986), Extension of a binaural cross-correlation
%       model by contralateral inhibition. I. Simulation of lateralization
%       for stationary signals, The Journal of the Acoustical Society of
%       America 80, 6, 1608-1622.
%
%   [2] Lindemann, W. (1986), Extension of a binaural cross-correlation
%       model by contralateral inhibition. II. The law of the first wave
%       front, The Journal of the Acoustical Society of America 80, 6,
%       1623-1630.
%
%   Further reading
%
%   Hummersone, C., Mason, R., Brookes, T. (2013), A comparison of
%       computational precedence models for source separation in
%       reverberant environments, The Journal of the Audio Engineering
%       Society 61, 7/8, 508-520.
%
%   See also IOSR.AUDITORY.XCORRLINDEMANN.

%   Copyright 2016 University of Surrey.

    %% check input

    assert(isequal(size(L),size(R)), 'iosr:lindemannInh:invalidInputs', 'L and R arrays must be the same size')
    assert(isscalar(fs), 'iosr:lindemannInh:invalidFs', 'FS must be a scalar')

    % default dim
    if nargin<5
        dim = find(size(L)>1,1,'first');
    end

    %% process

    % half-wave rectify
    L_l = hwr(L);
    R_l = hwr(R);

    % filter
    cutofffreq=800;
    [b,a] = butter(1,cutofffreq*2/fs);
    L_l = filter(b,a,L_l,[],dim);
    R_l = filter(b,a,R_l,[],dim);

    % inhibition parameter
    if nargin < 4
        c_inh = .3;
    else
        assert(isscalar(c_inh) & isnumeric(c_inh), 'iosr:lindemannInh:invalidCinh', 'c_inh must be a scalar');
        assert(c_inh>=0 || c_inh<=1, 'iosr:lindemannInh:invalidX', 'c_inh must be in the interval (0,1].')
    end

    % gain
    c_gamma_L = calc_gamma(L,dim);
    c_gamma_R = calc_gamma(R,dim);
    c_gamma = max([c_gamma_L(:); c_gamma_R(:)]);

    % apply parameters
    L_l = apply_gamma(L_l,c_inh,c_gamma);
    R_l = apply_gamma(R_l,c_inh,c_gamma);

    % restrict range
    L_l = clip(L_l);
    R_l = clip(R_l);

end

function y = hwr(x)
%HWR half-wave rectify

    y = max(x,0);

end

function gamma = calc_gamma(x,dim)
%CALC_GAMMA calculate the gamma

    gamma = sqrt(2).*iosr.dsp.rms(x,dim);

end

function y = apply_gamma(x,c_inh,c_gamma)
%APPLY_GAMMA apply gamma to achieve inhibition parameter

    y = (c_inh/c_gamma).*x;

end

function y = clip(x)
%CLIP clip input data to [0,1] interval

    y = x;
    y(y<0) = 0;
    y(y>1) = 1;

end


classdef m_47_IHM19_16p_4s < MARRMoT_model
% Class for hydrologic conceptual model: Forellenbach model

% Copyright (C) 2021 Clara Brandes, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.
%
% Model reference
% Brandes, C. (2020). Erstellung eines Konzeptionellen
% Hochwasserabfluss-modells für das Einzugsgebiet des Forellenbachs, NP
% Bayerischer Wald. MSc Thesis. Technische Univerisität Dresden, Germany.

    properties
        % model-specific attributes
    end
    methods

        % creator method
        function obj = m_47_IHM19_16p_4s()
            obj.numStores = 4;                                             % number of model stores
            obj.numFluxes = 18;                                            % number of model fluxes
            obj.numParams = 16;

            obj.JacobPattern  = [1,0,0,0;
                                 1,1,0,0;
                                 1,1,1,0;
                                 0,1,1,1];                                 % Jacobian matrix of model store ODEs

            obj.parRanges = [   2,     5;      % 01 SIMAX, maximum interception storage [mm]
                                0.9,   1;      % 02 A, splitting coeffcient for excess precipitation [-]
                                0.4,   0.95;   % 03 FF, forest fraction [-]
                                0.05,  5;      % 04 SMPMAX, maximum storage macropores [mm]
                                0,     1;      % 05 CQMP, runoff time parameter (fast/slow runnoff) first soil layer [1/d]
                                1,     5;      % 06 XQMP, runoff scale parameter first soil layer [-]
                              400,   600;      % 07 SS1MAX, maximum soil moisture storage first soil layer [mm]
                                0.3,   0.7;    % 08 FCCS1, field capacity as fraction of maximum storage first soil layer [-]
                                0,  1000;      % 09 CFS1, maximum infiltration rate first soil layer [mm/d]
                                0,    15;      % 10 XFS1, infiltration loss exponent first soil layer [-]
                                0,     1;      % 11 CQS1, runoff time parameter for (fast/slow runnoff) first soil layer [1/d]
                                1,     5;      % 12 XQS1, runoff scale parameter first soil layer [-]
                              300,   500;      % 13 SS2MAX, maximum soil moisture storage second soil layer [mm]
                                0,     1;      % 14 CQS2, runoff time parameter for (fast/slow runnoff) second soil layer [1/d]
                                1,     5;      % 15 XQS2, runoff scale parameter second soil layer [-]
                                0.01,  5];     % 16 D, Flow delay before surface runoff [d]

            obj.StoreNames = {"S1", "S2" "S3" "S4"};                        % Names for the stores
            obj.FluxNames  = {"ei",    "pex", "pexmp",  "pexs1", "fmp",...
                              "qexmp", "qmp", "pqexsl", "fs1",   "etas1",...
                              "qs1",   "q0",  "q0r",    "qmps1", "pc",...
                              "qh",    "qs2", "qgw"};                      % Names for the fluxes

            obj.FluxGroups.Ea = [1 10];                                    % Index or indices of fluxes to add to Actual ET
            obj.FluxGroups.Q  = [13 16 17 18];                             % Index or indices of fluxes to add to Streamflow
            obj.FluxGroups.GW = -18;                                       % Index of GW runoff flow.

        end

        % INITialisation function
        function obj = init(obj)
            % parameters
            theta   = obj.theta;
            delta_t = obj.delta_t;
            SIMAX   = theta(1);                         % maximum interception storage [mm]
            SMPMAX  = theta(4);                         % maximum storage macropores [mm]
            SS1MAX  = theta(7);                         % maximum soil moisture storage first soil layer [mm]
            SS2MAX  = theta(13);                        % maximum soil moisture storage second soil layer [mm]
            D       = theta(16);                        % Flow delay before surface runoff [d]

            % initial store values
            obj.S0 = 0.8 * [SIMAX;                   % Initial interception storage
                            SMPMAX;                  % Initial macropore storage
                            SS1MAX;                  % Initial soil moisture storage 1
                            SS2MAX];                 % Initial soil moisture storage 2

            % initialise the unit hydrographs and still-to-flow vectors
            uh_q0r = uh_4_full(D,delta_t);

            obj.uhs = {uh_q0r};
        end

        % MODEL_FUN are the model governing equations in state-space formulation
        function [dS, fluxes] = model_fun(obj, S)
            % parameters
            theta   = obj.theta;
            SIMAX   = theta(1);                         % maximum interception storage [mm]
            A       = theta(2);                         % splitting coeffcient for excess precipitation [-]
            FF      = theta(3);                         % forest fraction [-]
            SMPMAX  = theta(4);                         % maximum storage macropores [mm]
            CQMP    = theta(5);                         % runoff time parameter (fast/slow runnoff) first soil layer [1/d]
            XQMP    = theta(6);                         % runoff scale parameter first soil layer [-]
            SS1MAX  = theta(7);                         % maximum soil moisture storage first soil layer [mm]
            FCCS1   = theta(8);                         % field capacity coefficient fist soil layer [-]
            CFS1    = theta(9);                         % maximum infiltration rate first soil layer [-]
            XFS1    = theta(10);                        % infiltration loss exponent first soil layer [-]
            CQS1    = theta(11);                        % runoff time parameter for (fast/slow runnoff) first soil layer [1/d]
            XQS1    = theta(12);                        % runoff scale parameter first soil layer [-]
            SS2MAX  = theta(13);                        % maximum soil moisture storage second soil layer [mm]
            CQS2    = theta(14);                        % runoff time parameter for (fast/slow runnoff) second soil layer [1/d]
            XQS2    = theta(15);                        % runoff scale parameter second soil layer [-]

            % delta_t
            delta_t = obj.delta_t;

            % unit hydrographs and still-to-flow vectors
            uhs = obj.uhs;
            uh_q0r = uhs{1};

            % stores
            S1 = S(1);
            S2 = S(2);
            S3 = S(3);
            S4 = S(4);

            % climate input
            t = obj.t;                             % this time step
            climate_in = obj.input_climate(t,:);   % climate at this step
            P  = climate_in(1);
            Ep = climate_in(2);
            T  = climate_in(3);

            % fluxes functions
            flux_ei      = evap_1(S1,Ep,delta_t);
            flux_pex     = interception_1(P,S1,SIMAX);
            flux_pexmp   = split_1(A,flux_pex);
            flux_pexs1   = split_2(A,flux_pex);
            flux_fmp     = infiltration_3(flux_pexmp,S2,SMPMAX);
            flux_qexmp   = flux_pexmp - flux_fmp;
            flux_qmp     = interflow_3(CQMP,XQMP,S2,delta_t);
            flux_pqexs1  = flux_pexs1 + flux_qexmp;
            flux_fs1     = infiltration_7(CFS1,XFS1,S3,SS1MAX,flux_pqexs1);
            flux_etas1   = evap_23(FF,FCCS1,S3,SS1MAX,Ep,delta_t);
            flux_qs1     = interflow_9(S3,CQS1,FCCS1*SS1MAX,XQS1,delta_t);
            flux_q0      = flux_pqexs1 - flux_fs1;
            flux_q0r     = route(flux_q0, uh_q0r);
            flux_qmps1   = flux_qmp + flux_qs1;
            flux_pc      = infiltration_3(flux_qmps1,S4,SS2MAX);
            flux_qh      = flux_qmps1 - flux_pc;
            flux_qs2     = interflow_3(CQS2,XQS2,S4,delta_t);
            flux_qgw     = 0.0195;

            % stores ODEs
            dS1 = P - flux_ei - flux_pex;
            dS2 = flux_fmp - flux_qmp;
            dS3 = flux_fs1 - flux_etas1 - flux_qs1;
            dS4 = flux_pc - flux_qs2;

            % outputs
            dS = [dS1 dS2 dS3 dS4];
            fluxes = [flux_ei, flux_pex, flux_pexmp, flux_pexs1,...
                      flux_fmp, flux_qexmp, flux_qmp, flux_pqexs1,...
                      flux_fs1, flux_etas1, flux_qs1, flux_q0,...
                      flux_q0r, flux_qmps1, flux_pc , flux_qh,...
                      flux_qs2, flux_qgw];
        end

        % STEP runs at the end of every timestep.
        function obj = step(obj)
            % unit hydrographs and still-to-flow vectors
            uhs = obj.uhs;
            uh_q0r = uhs{1};

            % input fluxes to the unit hydrographs
            fluxes = obj.fluxes(obj.t,:);
            flux_q0   = fluxes(12);

            % update still-to-flow vectors using fluxes at current step and
            % unit hydrographs
            uh_q0r = update_uh(uh_q0r, flux_q0);
            obj.uhs = {uh_q0r};
        end
    end
end