classdef inversion_recovery < AbstractModel
%inversion_recovery: Compute a T1 map using Inversion Recovery data
%
% Assumptions:
% (1) Gold standard for T1 mapping
% (2) Infinite TR
%
% Inputs:
%   IRData      Inversion Recovery data (4D)
%   (Mask)      Binary mask to accelerate the fitting (OPTIONAL)
%
% Outputs:
%   T1          transverse relaxation time [ms]
%   b           arbitrary fit parameter (S=a + b*exp(-TI/T1))
%   a           arbitrary fit parameter (S=a + b*exp(-TI/T1))
%   idx         index of last polarity restored datapoint (only used for magnitude data)
%   res         Fitting residual
%
%
% Protocol:
%	IRData  [TI1 TI2...TIn] inversion times [ms]
%   TimingTable   [TR] repetition time [ms]
%
% Options:
%   method          Method to use in order to fit the data, based on whether complex or only magnitude data acquired.
%     'complex'     Complex dataset.
%     'magnitude'   Magnitude dataset.
%
%
%   fitModel        T1 fitting moddel.
%     'Barral'      Fitting equation: a+bexp(-TI/T1)
%     'General'     Fitting equation: c(1-2exp(-TI/T1)+exp(-TR/T1))
%
% Example of command line usage (see also showdemo inversion_recovery_batch):
%   Model = inversion_recovery;  % Create class from model
%   Model.Prot.IRData.Mat=[350.0000; 500.0000; 650.0000; 800.0000; 950.0000; 1100.0000; 1250.0000; 1400.0000; 1700.0000];
%   data = struct;  % Create data structure
%   data.MET2data ='IRData.mat';  % Load data
%   data.Mask = 'Mask.mat';
%   FitResults = FitData(data,Model); %fit data
%   FitResultsSave_mat(FitResults);
%
%       For more examples: qMRusage(inversion_recovery)
%
% Author: Ilana Leppert, 2017
%
% References:
%   Please cite the following if you use this module:
%       A robust methodology for in vivo T1 mapping. Barral JK, Gudmundson E, Stikov N, Etezadi-Amoli M, Stoica P, Nishimura DG. Magn Reson Med. 2010 Oct;64(4):1057-67. doi: 10.1002/mrm.22497.
%   In addition to citing the package:
%     Karakuzu A., Boudreau M., Duval T.,Boshkovski T., Leppert I.R., Cabana J.F.,
%     Gagnon I., Beliveau P., Pike G.B., Cohen-Adad J., Stikov N. (2020), qMRLab:
%     Quantitative MRI analysis, under one umbrella doi: 10.21105/joss.02343

properties (Hidden=true)
    onlineData_url = 'https://osf.io/cmg9z/download?version=3';
end

	properties
        MRIinputs = {'IRData','Mask'}; % input data required
        xnames = {'T1','rb','ra'}; % name of the fitted parameters
        voxelwise = 1; % voxel by voxel fitting?

        % fitting options
        st           = [  600    -1000      500 ]; % starting point
        lb           = [    0.0001   -10000        0.0001 ]; % lower bound
        ub           = [ 5000        0    10000 ]; % upper bound
        fx           = [    0        0        0 ]; % fix parameters

        % Protocol
        Prot = struct('IRData', struct('Format',{'TI(ms)'},'Mat',[350 500 650 800 950 1100 1250 1400 1700]'),...
                      'TimingTable', struct('Format',{{'TR(ms)'}},'Mat',2500)); %default protocol
        % Model options
        buttons = {'method',{'Magnitude','Complex'}, 'fitModel',{'Barral','General'}}; %selection buttons
        options = struct(); % structure filled by the buttons. Leave empty in the code

        % Simulation Options
        Sim_Single_Voxel_Curve_buttons = {'SNR',50,'T1',600,'M0',1000,'TR',3000,'FAinv',180,'FAexcite',90,'Update input variables','pushbutton'};%'FArefocus',180
        Sim_Optimize_Protocol_buttons = {'# of volumes',5,'Population size',100,'# of migrations',100};

    end

methods (Hidden=true)
% Hidden methods goes here.
end

    methods
        % -------------CONSTRUCTOR-------------------------------------------------------------------------
        function  obj = inversion_recovery()
            obj.options = button2opts(obj.buttons);
        end

        function obj = UpdateFields(obj)
            obj.Prot.IRData.Mat = sort(obj.Prot.IRData.Mat);
        end
        function xnew = SimOpt(obj,x,Opt)
            [ra,rb] = ComputeRaRb(obj,x,Opt);
            xnew = [Opt.T1 rb ra];

        end

        % -------------IR EQUATION-------------------------------------------------------------------------
        function Smodel = equation(obj, x)
            % Generates an IR signal based on fit parameters
            x = mat2struct(x,obj.xnames); % if x is a structure, convert to vector

            % equation
            Smodel = x.ra + x.rb * exp(-obj.Prot.IRData.Mat./x.T1);
            if (strcmp(obj.options.method, 'Magnitude'))
                Smodel = abs(Smodel);
            end
        end

        % -------------EXPLICIT IR EQUATION-------------------------------------------------------------------------
        function [ra,rb] = ComputeRaRb(obj,x,Opt)

            % Some sanity checks
            [ErrMsg]=[];

            for brkloop=1:1
                if Opt.TR < max(obj.Prot.IRData.Mat) %TR can't be less than max TI
                    txt=['The TR (' num2str(Opt.TR) ') cannot be less than max TI (' num2str(max(obj.Prot.IRData.Mat)),')'];
                    ErrMsg = txt; break
                end
                if Opt.T1 < 0 || Opt.T1 > 10000
                    txt='Choose a reasonable value for T1 (0-10000 s)';
                    ErrMsg = txt; break
                end
                if Opt.FAinv < 120 || Opt.FAinv > 220
                    txt='Choose a reasonable value for the inversion FA (120-220 deg)';
                    ErrMsg = txt; break
                end
                 if Opt.FAexcite < 50 || Opt.FAexcite > 120
                    txt='Choose a reasonable value for the excitation FA (50-120 deg)';
                    ErrMsg = txt; break
                end
            end
            if ~isempty(ErrMsg)
                if moxunit_util_platform_is_octave
                    errordlg(ErrMsg,'Input Error');
                else
                    Mode = struct('WindowStyle','modal','Interpreter','tex');
                    errordlg(ErrMsg,'Input Error', Mode);
                    error(ErrMsg);
                end
            end

            % equation for GRE-IR
            ra = Opt.M0 * (1-cos(Opt.FAinv*pi/180)*exp(-Opt.TR/Opt.T1))/(1-cos(Opt.FAinv*pi/180)*cos(Opt.FAexcite*pi/180)*exp(-Opt.TR/Opt.T1));
            rb = -Opt.M0 * (1-cos(Opt.FAinv*pi/180))/(1-cos(Opt.FAinv*pi/180)*cos(Opt.FAexcite*pi/180)*exp(-Opt.TR/Opt.T1));
            %Smodel = ra + rb * exp(-obj.Prot.IRData.Mat./x.T1);
            %if (strcmp(obj.options.method, 'Magnitude'))
            %    Smodel = abs(Smodel);
            %end
        end

        % -------------DATA FITTING-------------------------------------------------------------------------
        function FitResults = fit(obj,data)
            % Fits the data
            %

            data = data.IRData;

            switch obj.options.fitModel
                case 'Barral'
                    [T1,rb,ra,res,idx] = fitT1_IR(data,obj.Prot.IRData.Mat,obj.options.method);
                    FitResults.T1  = T1;
                    FitResults.rb  = rb;
                    FitResults.ra  = ra;
                    FitResults.res = res;
                    if (strcmp(obj.options.method, 'Magnitude'))
                        FitResults.idx = idx;
                    end
                case 'General'
                    approxFlag = 3; % Selects the equation c(1-2exp(-TI/T1)+exp(TR/T1))

                    params.TI = obj.Prot.IRData.Mat';
                    params.TR = obj.Prot.TimingTable.Mat;

                    if strcmp(obj.options.method, 'Magnitude')
                        % Make sure data vector is a column vector
                        data = data(:);

                        % Find the min of the data (which is nearest to
                        % signal null to flip
                        [~, minInd] = min(data);

                        % Signal inversion algorithm to fir the T1 curve
                        for ii = 1:2
                            % Fit the data by inverting the sign of the
                            % datapoints up until the TI where signal is
                            % null, then also without that point. The
                            % fit with the smallest residual is our best
                            % guess for up to where to flip the sign of the
                            % signal.
                            if ii == 1
                                % First, we set all elements up to and including
                                % the smallest element to minus
                                dataTmp = data.*[-ones(minInd,1); ones(length(data) - minInd,1)];
                            elseif ii == 2
                                % Second, we set all elements up to (not including)
                                % the smallest element to minus
                                dataTmp = data.*[-ones(minInd-1,1); ones(length(data) - (minInd-1),1)];
                            end
                            [fitVals{ii}, resnorm(ii)] = inversion_recovery.fit_lm(dataTmp, params, approxFlag);
                        end
                        [~,ind] = min(resnorm); % Index of the minimum residual will be which signal fit results to choose.
                        FitResults.T1 = fitVals{ind}.T1;
                        FitResults.ra = fitVals{ind}.ra;
                        FitResults.rb = fitVals{ind}.rb;
                        FitResults.res = resnorm(ind);
                        FitResults.idx = ind;
                    elseif strcmp(obj.options.method, 'Complex')
                        params.dataType = 'complex';
                        [fitVals, resnorm] = inversion_recovery.fit_lm(data(:), params, 3);
                        FitResults.T1 = fitVals.T1;
                        FitResults.ra = fitVals.ra;
                        FitResults.rb = fitVals.rb;
                        FitResults.res = resnorm;
                    end
            end
        end

        function plotModel(obj, FitResults, data)
            % Plots the fit
            %
            % :param FitResults: [struct] Fitting parameters
            % :param data: [struct] input data
            if nargin<2 || isempty(FitResults), FitResults = obj.st; end
            if exist('data','var')
                data = data.IRData;
                % plot
                plot(obj.Prot.IRData.Mat,data,'.','MarkerSize',15)
                hold on
                if (strcmp(obj.options.method, 'Magnitude'))
                    % plot the polarity restored data points
                    data_rest = -1.*data(1:FitResults.idx);
                    plot(obj.Prot.IRData.Mat(1:FitResults.idx),data_rest,'o','MarkerSize',5,'MarkerEdgeColor','b','MarkerFaceColor',[1 0 0])
                end
            end

            % compute model
            obj.Prot.IRData.Mat = linspace(min(obj.Prot.IRData.Mat),max(obj.Prot.IRData.Mat),100);
            Smodel = equation(obj, FitResults);

            % plot fitting curve
            plot(obj.Prot.IRData.Mat,Smodel,'Linewidth',3)
            hold off
            xlabel('Inversion Time [ms]','FontSize',15);
            ylabel('Signal','FontSize',15);
            legend('data', 'polarity restored', 'fit','Location','best')
            set(gca,'FontSize',15)
        end

        function [FitResults, data] = Sim_Single_Voxel_Curve(obj, x, Opt,display)
            % Simulates Single Voxel
            %
            % :param x: [struct] fit parameters
            % :param Opt.SNR: [struct] signal to noise ratio to use
            % :param display: 1=display, 0=nodisplay
            % :returns: [struct] FitResults, data (noisy dataset)

            if ~exist('display','var'), display = 1; end

            Smodel = equation(obj, x);
            sigma = max(abs(Smodel))/Opt.SNR;
            if (strcmp(obj.options.method, 'Magnitude'))
                data.IRData = ricernd(Smodel,sigma);
            else
                data.IRData = random('normal',Smodel,sigma);
            end
            FitResults = fit(obj,data);
            if display
                plotModel(obj, FitResults, data);
            end
        end

        function SimVaryResults = Sim_Sensitivity_Analysis(obj, OptTable, Opt)
            % SimVaryGUI
            SimVaryResults = SimVary(obj, Opt.Nofrun, OptTable, Opt);
        end

        function SimRndResults = Sim_Multi_Voxel_Distribution(obj, RndParam, Opt)
            % SimRndGUI
            SimRndResults = SimRnd(obj, RndParam, Opt);
        end

%         function schemeLEADER = Sim_Optimize_Protocol(obj,xvalues,Opt)
%             % schemeLEADER = Sim_Optimize_Protocol(obj,xvalues,nV,popSize,migrations)
%             % schemeLEADER = Sim_Optimize_Protocol(obj,obj.st,30,100,100)
%             % Optimize Inversion times
%             nV         = Opt.Nofvolumes;
%             popSize    = Opt.Populationsize;
%             migrations = Opt.Nofmigrations;
%
%             sigma  = .05;
%             TImax = 5000;
%             TImin = 50;
%             GenerateRandFunction = @() rand(nV,1)*(TImax-TImin)+TImin; % do not sort TI values... or you might fall in a local minima
%             CheckProtInBoundFunc = @(Prot) min(max(50,Prot),TImax);
%             % Optimize Protocol
%             [retVal] = soma_all_to_one(@(Prot) mean(SimCRLB(obj,Prot,xvalues,sigma)), GenerateRandFunction, CheckProtInBoundFunc, migrations, popSize, nV, obj.Prot.IRData.Mat(:,1));
%
%             % Generate Rest
%             schemeLEADER = retVal.schemeLEADER;
%
%             fprintf('SOMA HAS FINISHED \n')
%
%         end

    end

    % CLI-only implemented static methods. Can be called directly from
    % class - no object needed.
    methods(Static)
        function Mz = analytical_solution(params, seqFlag, approxFlag)
            %ANALYTICAL_SOLUTION  Analytical equations for the longitudinal magnetization of
            %steady-state inversion recovery experiments with either a gradient echo
            %(GRE-IR) or spin-echo (SE-IR) readouts.
            %   Reference: Barral, J. K., Gudmundson, E. , Stikov, N. , Etezadi?Amoli,
            %   M. , Stoica, P. and Nishimura, D. G. (2010), A robust methodology for
            %   in vivo T1 mapping. Magn. Reson. Med., 64: 1057-1067.
            %   doi:10.1002/mrm.22497
            %
            %   params: struct with the required parameters for the sequence and
            %   approximation. See below for list.
            %
            %   seqFlag: String. Either 'GRE-IR' or 'SE-IR'
            %   approxFlag: Integer between 1 and 4.
            %       1: General equation (no approximation).
            %       2: Ideal 180 degree pulse approximation of case 1.
            %       3: Ideal 90 degree pulse approximation of case 2, and readout term
            %          absorbed into constant.
            %       4: Long TR (TR >> T1) approximation of case 3.
            %
            %   **PARAMS PROPERTIES**
            %   All times in ms, all angles in degrees.
            %  'GRE-IR'
            %       case 1: T1, TR, TI, EXC_FA, INV_FA, constant (optional)
            %       case 2: T1, TR, TI, EXC_FA, constant (optional)
            %       case 3: T1, TR, TI, constant (optional)
            %       case 4: T1, TI, constant (optional)
            %
            %  'SE-IR'
            %       case 1: Same as 'GRE-IR' case + SE_FA, TE
            %       case 2: Same as 'GRE-IR' case + TE
            %       case 3: Same as 'GRE-IR' case + TE
            %       case 4: Same as 'GRE-IR' case
            %

            Mz = ir_equations(params, seqFlag, approxFlag);

        end

        function [Mz, Msig] = bloch_sim(params)
            %BLOCH_SIM Bloch simulations of the GRE-IR pulse sequence.
            % Simulates 100 spins params.Nex repetitions of the IR pulse
            % sequences.
            %
            % params: Struct with the following fields:
            %   INV_FA: Inversion pulse flip angle in degrees.
            %   EXC_FA: Excitation pulse flip angle in degrees.
            %   TI: Inversion time (ms).
            %   TR: Repetition time (ms).
            %   TE: Echo time (ms).
            %   T1: Longitudinal relaxation time (ms).
            %   T2: Transverse relaxation time (ms).
            %   Nex: Number of excitations
            %
            %   (optional
            %       df: Off-resonance frequency of spins relative to excitation pulse (in Hz)
            %       crushFlag: Numeric flag for perfect spoiling (1) or partial spoiling (2).
            %       partialDephasing: Partial dephasing fraction (between [0, 1]). 1 = no dephasing, 0 = complete dephasing (sele
            %       inc: Phase spoiling increment in degrees.
            %
            % Outputs:
            %   Mz: Longitudinal magnetization at time TI (prior to excitation pulse).
            %   Msig: Complex signal produced by the transverse magnetization at time TE after excitation.
            %

            %% Setup parameters
            %

            alpha = deg2rad(params.INV_FA);
            beta  = deg2rad(params.EXC_FA);
            TI = params.TI;
            TR = params.TR;
            T1 = params.T1;

            TE = params.TE;
            T2 = params.T2;

            Nex = params.Nex;

            %% Optional parameers

            if isfield(params, 'df')
                df = params.df;
            else
                df = 0;
            end

            if isfield(params, 'crushFlag')
                crushFlag = params.crushFlag;
            else
                crushFlag = 1;
            end

            if isfield(params, 'partialDephasing')
                partialDephasing = params.partialDephasing;
            else
                partialDephasing = 1;
            end

            if isfield(params, 'inc')
                inc = deg2rad(params.inc);
            else
                inc = 0;
            end

            %% Simulate for every TI's
            %

            for ii = 1:length(TI)

                [Msig(ii), Mz(ii)] = ir_blochsim(                  ...
                                                 alpha,            ...
                                                 beta,             ...
                                                 TI(ii),           ...
                                                 T1,               ...
                                                 T2,               ...
                                                 TE,               ...
                                                 TR,               ...
                                                 crushFlag,        ...
                                                 partialDephasing, ...
                                                 df,               ...
                                                 Nex,              ...
                                                 inc               ...
                                                 );

            end

        end

        function [fitVals, resnorm] = fit_lm(data, params, approxFlag)
            % FIT_LM Levenberg-Marquardt fitting of GRE-IR data.
            %
            % data: Array for a single voxel (length = #TI)
            % params: Properties TI and TR (except for approxFlag = 4,
            %         where only TI is needed)
            % approxFlag: Same flags numbering & equations as for the
            %             analytical_solution class method.

            switch approxFlag
                case 1
                    TI = params.TI;
                    TR = params.TR;

                    %    [constant, T1, EXC_FA, INV_FA]
                    x0 = [1, 1000, 90, 180];

                    options.Algorithm = 'levenberg-marquardt';
                    options.Display = 'off';

                    [x, resnorm] = lsqnonlin(@(x)ir_loss_func_1(x, TR, TI, data), x0, [], [], options);

                    fitVals.INV_FA = x(4);
                    fitVals.EXC_FA = x(3);
                    fitVals.T1 = x(2);
                    fitVals.c = x(1);
                case 2
                    TI = params.TI;
                    TR = params.TR;

                    %    [constant, T1, EXC_FA]
                    x0 = [1, 1000, 90];

                    options.Algorithm = 'levenberg-marquardt';
                    options.Display = 'off';

                    [x, resnorm] = lsqnonlin(@(x)ir_loss_func_2(x, TR, TI, data), x0, [], [], options);

                    fitVals.EXC_FA = x(3);
                    fitVals.T1 = x(2);
                    fitVals.c = x(1);

                case 3
                    TI = params.TI;
                    TR = params.TR;

                    % Find the min of the data
                    [~, minInd] = min(abs(data));

                    T1est = -TI(minInd)/log(1/2); % Estimate from null (or min) of data and the equation in Case 4. Used as the first guess for the fitting point.

                    if isfield(params, 'dataType') && strcmp(params.dataType,'complex')
                        % Fit comlex data
                        %    [constant, T1]
                        if max(abs(data)) > 0
                            dataNorm = data/max(abs(data)); % Normalize the data to fit with the initial constant guess of 1.
                        else
                            dataNorm = data;
                        end

                        % Initial fit estimates
                        %    [Re(constant), Im(constant), T1)]
                        x0 = [1, 0, T1est];

                        options.Algorithm = 'trust-region-reflective';
                        options.Display = 'off';

                        % Bound the fitting variables to -2 to 2
                        % (constant) and 0 to 5000 (T1 in ms).
                        [x, resnorm] = lsqnonlin(@(x)ir_loss_func_3(x, TR, TI, dataNorm(:)', params.dataType), x0, [-2, -2, 0], [2, 2, 5000], options);

                        fitVals.T1 = x(3);

                        % ra and rb calculation from Equation 3
                        if max(abs(data)) > 0
                            fitVals.ra = (x(1)+1i*x(2))*max(abs(data)) * (1 + exp(-TR/fitVals.T1));
                            fitVals.rb = -2*(x(1)+1i*x(2))*max(abs(data));
                        else
                            fitVals.ra = (x(1)+1i*x(2)) * (1 + exp(-TR/fitVals.T1));
                            fitVals.rb = -2*(x(1)+1i*x(2));
                        end
                    else
                        % Fit magnitude data
                        %    [constant, T1]
                        x0 = [1, T1est];
                        if max(abs(data)) > 0
                            dataNorm = data/max(abs(data)); % Normalize the data to fit with the initial constant guess of 1.
                        else
                            dataNorm = data;
                        end

                        options.Algorithm = 'trust-region-reflective';
                        options.Display = 'off';

                        % Bound the fitting variables to -2 to 2
                        % (constant) and 0 to 5000 (T1 in ms).
                        [x, resnorm] = lsqnonlin(@(x)ir_loss_func_3(x, TR, TI, dataNorm(:)'), x0, [0, 0], [2, 5000], options);

                        fitVals.T1 = x(2);

                        % ra and rb calculation from Equation 3
                        if max(abs(data)) > 0
                            fitVals.ra = x(1)*max(abs(data)) * (1 + exp(-TR/fitVals.T1));
                            fitVals.rb = -2*x(1)*max(abs(data));
                        else
                            fitVals.ra = x(2) * (1 + exp(-TR/fitVals.T1));
                            fitVals.rb = -2*x(2);
                        end

                    end

                case 4
                    TI = params.TI;
                    %    [constant, T1]
                    x0 = [1, 1000];

                    options.Algorithm = 'levenberg-marquardt';
                    options.Display = 'off';

                    [x, resnorm] = lsqnonlin(@(x)ir_loss_func_4(x, TI, data), x0, [], [], options);

                    fitVals.T1 = x(2);
                    fitVals.c = x(1);
            end

        end

    end

    methods(Access = protected)
        function obj = qMRpatch(obj,loadedStruct, version)
            obj = qMRpatch@AbstractModel(obj,loadedStruct, version);
            % 2.0.10
            if checkanteriorver(version,[2 0 10])
                % Update buttons for simulation
                snrValue = obj.Sim_Single_Voxel_Curve_buttons{2};
                obj.Sim_Single_Voxel_Curve_buttons = {'SNR' snrValue 'T1' 600 'M0' 1000 'TR' 3000 'FAinv' 180 'FAexcite' 90 'Update input variables' 'pushbutton'};
            end
             if checkanteriorver(version,[2 4 1])
                % Update buttons for simulation
                obj.Prot = struct('IRData', struct('Format',{'TI(ms)'},'Mat',[350 500 650 800 950 1100 1250 1400 1700]'),...
                                  'TimingTable', struct('Format',{{'TR(ms)'}},'Mat',2500)); %default protocol
                % Model options
                obj.buttons = {'method',{'Magnitude','Complex'}, 'fitModel',{'Barral','General'}}; %selection buttons
                obj.options = button2opts(obj.buttons);
            end
        end
    end

end


function [B,L] = ordered_outline(F)
  % ORDERED_OUTLINE Find outline (boundary) edges of mesh
  %
  % [B,L] = outline(F)
  %
  % Input:
  %   F  #F by 3 face list of indices
  % Outputs:
  %   B  #B by 1 list of outline edges
  %   L  #loops+1 by 1 list of boundary loop start indices into B, the last
  %     entries is (by tradition) always the numel of B + 1
  %
  % Example:
  %   V = [0 0; 1 0; 1 1 ; 0 1; 4 0; 4 4; 0 4];
  %   F = [1 2 3; 1 3 4; 5 6 7];
  %   [B,L] = ordered_outline(F);
  %   hold on
  %   for l = 1:(numel(L)-1)
  %     plot(V([B(L(l):(L(l+1)-1)) B(L(l))],1),V([B(L(l):(L(l+1)-1)) B(L(l))],2));
  %   end
  %   hold off
  %
  % See also: outline
  %

  if size(F,2) == 2
    O = F;
  else
    O = outline(F);
  end
  % determine uniqueness of indices
  [u,m,n] = unique(O(:),'rows');
  % determine counts for each unique edge
  counts = accumarray(n(:), 1);
  % all counts should be 2
  assert(all(counts == 2));

  % number of outline edges
  no = size(O,1);

  % list of boundary
  B = [];
  L = [];
  unseen = true(no,1);
  while any(unseen)
    L = [L numel(B)+1];
    % find first unseen index
    f = find(unseen,1);
    % append start vertex to boundary
    next = O(f,1);
    % keep track of start
    start = next;
    unseen(f) = false;
    B = [B next];
    next = O(f,2);
    % loop until back at start
    while next ~= start
      B = [B next];
      % try to find next as source
      f = find(unseen & O(:,1) == next);
      if isempty(f)
        % try to find next as dest
        f = find(unseen & O(:,2) == next);
        assert(~isempty(f));
        next = O(f,1);
      else
        next = O(f,2);
      end
      unseen(f) = false;
    end
  end
  L = [L numel(B)+1];

end


%============================================================================
% Copyright (C) 2015, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

classdef DCM_IMU < handle
% DCM_IMU Implementation of Hyyti's IMU algorithm
%
%   If you use the algorithm in any scientific context, please cite:
%   Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs,"
%   International Journal of Navigation and Observation, vol. 2015, Article ID 503814, 18 pages, 2015.
%   http://dx.doi.org/10.1155/2015/503814
%
%   Date          Author          Notes
%   1/12/2015     Heikki Hyyti    Initial release

    %% Public properties
    properties (Access = public)
        g0 = 9.8189;                % gravitation around Helsinki, Finland (change according to your area)
        state = [0 0 1 0 0 0]';     % States are lowest row of rotation matrix and gyroscope x y and z biases
                                    % (C_31, C_32, C_33, w_b1, w_b2, w_b3)
        q_dcm2 = 0.1^2;             % estimated variance of dcm states (gyro variance per second)
        q_gyro_bias2 = 0.0001^2;    % very small number to make bias change slowly
        r_acc2 = 0.5^2;             % variance of calibrated accelerometer (g-component)
        r_a2 = 10^2;                % large variance for some unknown acceleration (acc = a + g)
        q_dcm2_init = 1^2;          % initial variance of dcm states (for attitude estimation)
        q_gyro_bias2_init = 0.1^2;  % initial variance of bias states (for bias estimator)
        a = zeros(3,1);             % estimated non-gravitational accelerations
        yaw = 0;                    % Yaw angle around z axis (in ZYX convention)
        pitch = 0;                  % Pitch angle around y axis
        roll = 0;                   % Roll angle around x axis
        P = [];                     % estimate covariance (these are initialized in constructor below)
        H = [];                     % observation model (static)
        Q = [];                     % proces noise covariance (static part)
        first_row = [1 0 0]';       % first row of of the rotation matrix (for yaw angle estimate)
    end

    %% Public methods
    methods (Access = public)
        function obj = DCM_IMU(varargin)
            updateP = true;
            for i = 1:2:nargin
                if  strcmp(varargin{i}, 'Gravity'), obj.g0 = varargin{i+1};
                elseif  strcmp(varargin{i}, 'State'), obj.state = varargin{i+1};
                elseif  strcmp(varargin{i}, 'Covariance'), obj.P = varargin{i+1}; updateP = false;
                elseif  strcmp(varargin{i}, 'DCMVariance'), obj.q_dcm2 = varargin{i+1};
                elseif  strcmp(varargin{i}, 'BiasVariance'), obj.q_gyro_bias2 = varargin{i+1};
                elseif  strcmp(varargin{i}, 'InitialDCMVariance'), obj.q_dcm2_init = varargin{i+1};
                elseif  strcmp(varargin{i}, 'InitialBiasVariance'), obj.q_gyro_bias2_init = varargin{i+1};
                elseif  strcmp(varargin{i}, 'MeasurementVariance'), obj.r_acc2 = varargin{i+1};
                elseif  strcmp(varargin{i}, 'MeasurementVarianceVariableGain'), obj.r_a2 = varargin{i+1};
                else error('Invalid argument');
                end
            end;

            if (updateP), obj.P = [obj.q_dcm2_init*eye(3), zeros(3,3); zeros(3,3), obj.q_gyro_bias2_init*eye(3)]; end;
            obj.H = [eye(3)*obj.g0, zeros(3,3)];
            obj.Q = [obj.q_dcm2*eye(3), zeros(3,3); zeros(3,3) obj.q_gyro_bias2*eye(3)];
        end
        function obj = UpdateIMU(obj, Gyroscope, Accelerometer, SamplePeriod)
            x = obj.state;
            x_last = x;
            Q_ = SamplePeriod^2 * obj.Q; %Process noise covariance with time dependent noise

            % control input (angular velocities from gyroscopes)
            if (size(Gyroscope,1) == 3), u = Gyroscope;
            else u = Gyroscope';
            end

            % "rotation operators"
            C3X = [0 -x(3) x(2); x(3) 0 -x(1); -x(2) x(1) 0];
            UX = [0 -(u(3)-x(6)) u(2)-x(5);
                u(3)-x(6) 0 -(u(1)-x(4));
                -(u(2)-x(5)) u(1)-x(4) 0];

            % Model generation
            A = [zeros(3,3) -SamplePeriod*C3X; zeros(3,6)];
            B = [SamplePeriod*C3X; zeros(3,3)];
            F = eye(6) + [-SamplePeriod*UX, -SamplePeriod*C3X; zeros(3,6)];

            % Kalman a priori prediction
            x_predict = x + A*x + B*u;
            P_predict = F * obj.P * F' + Q_;

            % measurements/observations (acceleromeres)
            if (size(Accelerometer,1) == 3), z = Accelerometer;
            else z = Accelerometer';
            end

            % recompute R using the error between acceleration and the model of g
            % (estimate of the magnitude of a0 in a = a0 + g)
            a_predict = z - x_predict(1:3)*obj.g0;
            a_len = sqrt(a_predict'*a_predict);
            R = (a_len*obj.r_a2 + obj.r_acc2)*eye(3);

            % Kalman innovation
            y = z - obj.H*x_predict;
            S = obj.H * P_predict * obj.H' + R;

            % Kalman gain
            K = P_predict * obj.H' / S;

            % update a posteriori
            x = x_predict + K * y;

            % update a posteriori covariance
            IKH = eye(6) - K*obj.H;
            obj.P = IKH * P_predict * IKH' + K * R * K'; % for using any K

            % normalization of x & P (divide by DCM vector length)
            dcm_vector_length = sqrt(x(1)^2 + x(2)^2 + x(3)^2);
            J_33 = [x(2)^2 + x(3)^2,    -x(1)*x(2),         -x(1)*x(3); ...
                    -x(1)*x(2),         x(1)^2 + x(3)^2,    -x(2)*x(3); ...
                    -x(1)*x(3),         -x(2)*x(3),         x(1)^2 + x(2)^2];
            J = [ J_33 / (dcm_vector_length^3), zeros(3,3); zeros(3,3), eye(3)];

            % Laplace approximation of normalization function for x to P, J = Jacobian(f,x)
            % P_new = E[J*(x-x0)*(x-x0)'*J'] = J*E[(x-x0)*(x-x0)']*J' = J*P*J'
            obj.P = J*obj.P*J';
            x(1:3) = x(1:3) ./ dcm_vector_length;
            obj.state = x;

            % compute Euler angles (not exactly a part of the extended Kalman filter)
            % yaw integration through full rotation matrix
            u_nb = u - x(4:6);
            if (true)
                % Fill rotation matrix from angular values

                % cy = cos(obj.yaw); %old angles (last state before integration)
                % sy = sin(obj.yaw);
                % cp = cos(obj.pitch);
                % sp = sin(obj.pitch);
                % cr = cos(obj.roll);
                % sr = sin(obj.roll);

                % % compute needed parts of rotation matrix R
                % R11 = cy*cp;
                % R12 = cy*sp*sr-sy*cr;
                % R13 = cy*sp*cr+sy*sr;
                % R21 = sy*cp;
                % R22 = sy*sp*sr+cy*cr;
                % R23 = sy*sp*cr-cy*sr;

                % compute needed parts of rotation matrix R using state x and yaw
                cy = cos(obj.yaw); %old yaw angle (last state before integration)
                sy = sin(obj.yaw);
                d = sqrt(x_last(2)^2 + x_last(3)^2);
                d_inv = 1 / d;

                % compute needed parts of rotation matrix R (state and angle based version, equivalent with the commented version above)
                R11 = cy * d;
                R12 = -(x_last(3)*sy + x_last(1)*x_last(2)*cy) * d_inv;
                R13 = (x_last(2)*sy - x_last(1)*x_last(3)*cy) * d_inv;
                R21 = sy * d;
                R22 = (x_last(3)*cy - x_last(1)*x_last(2)*sy) * d_inv;
                R23 = -(x_last(2)*cy + x_last(1)*x_last(3)*sy) * d_inv;

                % update needed parts of R for yaw computation
                R11_new = R11 + SamplePeriod*(u_nb(3)*R12 - u_nb(2)*R13);
                R21_new = R21 + SamplePeriod*(u_nb(3)*R22 - u_nb(2)*R23);

                obj.yaw = atan2(R21_new,R11_new);
            else
                % alternative method estimating the whole rotation matrix
                % integrate full rotation matrix (using first row estimate in memory)
                x1 = obj.first_row + SamplePeriod*UX'*obj.first_row; %rotate x1 by x1 x u_nb
                x2 = C3X * x1; %second row x2 = (state x x1)
                x2 = x2 ./ sqrt(x2(1)^2 + x2(2)^2 + x2(3)^2); % normalize length of the second row
                x1 = C3X' * x2; %recalculate first row x1 = (x2 * state) (ensure perpendicularity)
                obj.first_row = x1 ./ sqrt(x1(1)^2 + x1(2)^2 + x1(3)^2); % normalize length
                obj.yaw = atan2(x2(1),obj.first_row(1));
            end

            %compute new pitch and roll angles from a posteriori states
            obj.pitch = asin(-x(1));
            obj.roll = atan2(x(2),x(3));

            % save the estimated non-gravitational acceleration
            obj.a = z - x(1:3)*obj.g0; % acceleration estimate (g reduced)
        end
    end
end


function [y,D,nz] = ksvddenoise(params,msgdelta)
%KSVDDENOISE K-SVD denoising.
%  [Y,D] = KSVDDENOISE(PARAMS) denoises the specified (possibly
%  multi-dimensional) signal using K-SVD denoising. Y is the denoised
%  signal and D is the trained dictionary produced by K-SVD.
%
%  [Y,D] = KSVDDENOISE(PARAMS,MSGDELTA) specifies the frequency of message
%  printing during the process. MSGDELTA should be a positive number
%  representing the interval in seconds between messages. A zero or
%  negative value cancels all messages. Default is MSGDELTA=5.
%
%  [Y,D,NZ] = KSVDDENOISE(...) also returns the average number of non-zero
%  coefficients in the representations of the denoised blocks.
%
%
%  Required fields in PARAMS:
%  --------------------------
%
%    'x' - Noisy signal.
%      The signal to denoise (can be multi-dimensional). Should be of type
%      double, and (for PSNR computations) with values within [0,1] (to
%      specify a different range, see parameter 'maxval' below).
%
%    'blocksize' - Size of block.
%      Indicates the size of the blocks to operate on. Should be either an
%      array of the form [N1 N2 ... Np], where p is the number of
%      dimensions of x, or simply a scalar N, representing the square block
%      [N N ... N]. See parameter 'stepsize' below to specify the amount of
%      overlap between the blocks.
%
%    'dictsize' - Size of dictionary to train.
%      Specifies the number of dictionary atoms to train by K-SVD.
%
%    'psnr' / 'sigma' - Noise power.
%      Specifies the noise power in dB (psnr) or the noise standard
%      deviation (sigma), used to determine the target error for
%      sparse-coding each block. If both fields are present, sigma is used
%      unless the field 'noisemode' is specified (below). When specifying
%      the noise power in psnr, make sure to set the 'maxval' parameter
%      as well (below) if the signal values are not within [0,1].
%
%    'trainnum' - Number of training blocks.
%      Specifies the number of training blocks to extract from the noisy
%      signal for K-SVD training.
%
%
%  Optional fields in PARAMS:
%  --------------------------
%
%    'initdict' - Initial dictionary.
%      Specifies the initial dictionary for the K-SVD training. Should be
%      either a matrix of size NxL where N=(N1*N2*...*Np), the string
%      'odct' to specify the overcomplete DCT dictionary, or the string
%      'data' to initialize using random signal blocks. When a matrix is
%      specified for 'initdict', L must be >= dictsize, and in this case
%      the dictionary is initialized using the first dictsize columns from
%      initdict. By default, initdict='odct'.
%
%    'stepsize' -  Interval between neighboring blocks.
%      Specifies the interval (in pixels/voxels) between neighboring blocks
%      to denoise in the OMP denoising step. By default, all overlapping
%      blocks are denoised and averaged. This can be changed by specifying
%      an alternate stepsize, as an array of the form [S1 S2 ... Sp] (where
%      p is the number of dimensions of x). This sets the distance between
%      neighboring blocks to be Si in the i-th dimension. Stepsize can also
%      be a scalar S, corresponding to the step size [S S ... S]. Each Si
%      must be >= 1, and, to ensure coverage of the entire noisy signal,
%      size(x,i)-Ni should be a multiple of Si for all i. The default
%      stepsize is 1.
%
%    'iternum' - Number of K-SVD iterations.
%      Specifies the number of K-SVD training iterations to perform. If not
%      specified, the default is 10.
%
%    'maxval' - Maximal intensity value.
%      Specifies the range of the signal values. Used to determine the
%      noise standard deviation when the noise power is specified in psnr.
%      By default, the signal values are assumed to be within [0,1]. When
%      'maxval' is specified, this range changes to [0,maxval].
%
%    'memusage' - Memory usage.
%      This parameter controls memory usage of the function. 'memusage'
%      should be one of the strings 'high', 'normal' (default) or 'low'.
%      When 'memusage' is specified, both KSVD and OMPDENOISE are invoked
%      using their corresponding memusage setting. Additionallly, when set
%      to 'low', the use of the functions OMPDENOISE2 and OMPDENOISE3 to
%      accelerate denoising of 2-D and 3-D signals is disabled, reverting
%      to the function OMPDENOISE. Note that the 'low' setting will
%      significantly increase runtime.
%
%
%  Optional fields in PARAMS - advanced:
%  -------------------------------------
%
%    'noisemode' - Noise power mode.
%      Specifies whether the 'psnr' or 'sigma' fields should be used to
%      determine the noise power. This is useful when both fields are
%      present in PARAMS. 'noisemode' should be one of the string 'psnr' or
%      'sigma'. If it is not present, and both fields are specified,
%      'sigma' is used.
%
%    'gain' - Noise gain.
%      A positive value (usually near 1) controlling the target error for
%      sparse-coding each block. When gain=1, the target error is precisely
%      the value derived from the psnr/sigma fields. When gain is different
%      from 1, the target error is multiplied by this value. The default
%      value is gain = 1.15.
%
%    'lambda' - Weight of the input signal.
%      Specifies the relative weight attributed to the noisy input signal
%      in determining the output. The default value is 0.1*(maxval/sigma),
%      where sigma is the standard deviation of the noise. See function
%      OMPDENOISE for more information.
%
%    'maxatoms' - Maximal number of atoms.
%      This parameter can be used to specify a hard limit on the number of
%      atoms used to sparse-code each block. Default value is
%      prod(blocksize)/2, i.e. half the number of samples in a block. See
%      function OMP2 for more information.
%
%    'exact' - Exact K-SVD update.
%      Specifies whether the exact or approximate dictionary update should
%      be used in the K-SVD training. By default, the approximate
%      computation is used. However, specifying a nonzero value for 'exact'
%      causes the exact computation to be used instead. See function KSVD
%      for more information.
%
%
%   Summary of all fields in PARAMS:
%   --------------------------------
%
%   Required:
%     'x'                      signal to denoise
%     'blocksize'              size of block to process
%     'dictsize'               size of dictionary to train
%     'psnr' / 'sigma'         noise power in dB / standard deviation
%     'trainnum'               number of training signals
%
%   Optional (default values in parenthesis):
%     'initdict'               initial dictionary ('odct')
%     'stepsize'               distance between neighboring blocks (1)
%     'iternum'                number of training iterations (10)
%     'maxval'                 maximal intensity value (1)
%     'memusage'               'low, 'normal' or 'high' ('normal')
%     'noisemode'              'psnr' or 'sigma' ('sigma')
%     'gain'                   noise gain (1.15)
%     'lambda'                 weight of input signal (0.1*maxval/sigma)
%     'maxatoms'               max # of atoms per block (prod(blocksize)/2)
%     'exact'                  exact update instead of approximate (0)
%
%
%  References:
%  [1] M. Elad and M. Aharon, "Image Denoising via Sparse and Redundant
%      representations over Learned Dictionaries", the IEEE Trans. on Image
%      Processing, Vol. 15, no. 12, pp. 3736-3745, December 2006.
%
%  See also KSVD, OMPDENOISE, OMP2.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

%%%%% parse input parameters %%%%%
addpath('ompbox');

x = params.x;
blocksize = params.blocksize;
trainnum = params.trainnum;
dictsize = params.dictsize;

p = ndims(x);
if (p==2 && any(size(x)==1) && length(blocksize)==1)
  p = 1;
end

% blocksize %
if (numel(blocksize)==1)
  blocksize = ones(1,p)*blocksize;
end

% maxval %
if (isfield(params,'maxval'))
  maxval = params.maxval;
else
  maxval = 1;
  params.maxval = maxval;
end

% gain %
if (isfield(params,'gain'))
  gain = params.gain;
else
  gain = 1.15;
  params.gain = gain;
end

% msgdelta %
if (nargin<2)
  msgdelta = 5;
end

verbose = 't';
if (msgdelta <= 0)
  verbose='';
  msgdelta = -1;
end

% initial dictionary %

if (~isfield(params,'initdict'))
  params.initdict = 'odct';
end

if (isfield(params,'initdict') && ischar(params.initdict))
  if (strcmpi(params.initdict,'odct'))
    params.initdict = odctndict(blocksize,dictsize,p);
  elseif (strcmpi(params.initdict,'data'))
    params = rmfield(params,'initdict');    % causes initialization using random examples
  else
    error('Invalid initial dictionary specified.');
  end
end

if (isfield(params,'initdict'))
  params.initdict = params.initdict(:,1:dictsize);
end

% noise mode %
if (isfield(params,'noisemode'))
  switch lower(params.noisemode)
    case 'psnr'
      sigma = maxval / 10^(params.psnr/20);
    case 'sigma'
      sigma = params.sigma;
    otherwise
      error('Invalid noise mode specified');
  end
elseif (isfield(params,'sigma'))
  sigma = params.sigma;
elseif (isfield(params,'psnr'))
  sigma = maxval / 10^(params.psnr/20);
else
  error('Noise strength not specified');
end

params.Edata = sqrt(prod(blocksize)) * sigma * gain;   % target error for omp
params.codemode = 'error';

params.sigma = sigma;
params.noisemode = 'sigma';

% make sure test data is not present in params
if (isfield(params,'testdata'))
  params = rmfield(params,'testdata');
end

%%%% create training data %%%

ids = cell(p,1);
if (p==1)
  ids{1} = reggrid(length(x)-blocksize+1, trainnum, 'eqdist');
else
  [ids{:}] = reggrid(size(x)-blocksize+1, trainnum, 'eqdist');
end
params.data = sampgrid(x,blocksize,ids{:});
params.data = remove_dc(params.data,'columns');

%%%%% KSVD training %%%%%

if (msgdelta>0)
  disp('KSVD training...');
end
D = ksvd(params,verbose,msgdelta);

%%%%%  denoise the signal  %%%%%

if (~isfield(params,'lambda'))
  params.lambda = maxval/(10*sigma);
end

params.dict = D;

if (msgdelta>0)
  disp('OMP denoising...');
end

% call the appropriate ompdenoise function
if (p>3  || (isfield(params,'memusage') && strcmpi(params.memusage,'low')))
  [y,nz] = ompdenoise(params,msgdelta);
elseif (p==1)
  [y,nz] = ompdenoise1(params,msgdelta);
elseif (p==2)
  [y,nz] = ompdenoise2(params,msgdelta);
else
  [y,nz] = ompdenoise3(params,msgdelta);
end

return;


function [dataout] = ft_denoise_dssp(cfg, datain)

% FT_DENOISE_DSSP implements a dual signal subspace projection algorithm
% to suppress interference outside a predefined source region of
% interest. It is based on: Sekihara et al. J. Neural Eng. 2016 13(3), and
% Sekihara et al. J. Neural Eng. 2018 15(3).
%
% Use as
%   dataout = ft_denoise_dssp(cfg, datain)
% where cfg is a configuration structure that contains
%   cfg.channel          = Nx1 cell-array with selection of channels (default = 'all'), see FT_CHANNELSELECTION for details
%   cfg.trials           = 'all' or a selection given as a 1xN vector (default = 'all')
%   cfg.sourcemodel      = structure, source model with precomputed leadfields, see FT_PREPARE_LEADFIELD
%   cfg.dssp             = structure with parameters that determine the behavior of the algorithm
%   cfg.dssp.n_space     = 'all', or scalar. Number of dimensions for the
%                          initial spatial projection.
%   cfg.dssp.n_in        = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field inside the ROI.
%   cfg.dssp.n_out       = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field outside the ROI.
%   cfg.dssp.n_intersect = scalar (default = 0.9). Number of dimensions (if
%                          value is an integer>=1), or threshold for the
%                          included eigenvalues (if value<1), determining
%                          the dimensionality of the intersection.
%
% See also FT_DENOISE_PCA, FT_DENOISE_SYNTHETIC, FT_DENOISE_TSR

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar    datain
ft_preamble provenance datain
ft_preamble trackconfig

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  % do not continue function execution in case the outputfile is present and the user indicated to keep it
  return
end

% check the input data
datain = ft_checkdata(datain, 'datatype', {'raw'}); % FIXME how about timelock and freq?

cfg = ft_checkconfig(cfg, 'renamed', {'hdmfile', 'headmodel'});
cfg = ft_checkconfig(cfg, 'renamed', {'vol',     'headmodel'});
cfg = ft_checkconfig(cfg, 'renamed', {'grid',    'sourcemodel'});

% get the options
cfg.trials       = ft_getopt(cfg, 'trials',  'all', 1);
cfg.channel      = ft_getopt(cfg, 'channel', 'all');
cfg.sourcemodel  = ft_getopt(cfg, 'sourcemodel');
cfg.dssp         = ft_getopt(cfg, 'dssp');         % sub-structure to hold the parameters
cfg.dssp.n_space = ft_getopt(cfg.dssp, 'n_space', 'all'); % number of spatial components to retain from the Gram matrix
cfg.dssp.n_in    = ft_getopt(cfg.dssp, 'n_in', 'all');    % dimensionality of the Bin subspace to be used for the computation of the intersection
cfg.dssp.n_out   = ft_getopt(cfg.dssp, 'n_out', 'all');   % dimensionality of the Bout subspace to be used for the computation of the intersection
cfg.dssp.n_intersect = ft_getopt(cfg.dssp, 'n_intersect', 0.9); % dimensionality of the intersection
cfg.output       = ft_getopt(cfg, 'output', 'original');

% select channels and trials of interest, by default this will select all channels and trials
tmpcfg = keepfields(cfg, {'trials', 'channel', 'showcallinfo'});
datain = ft_selectdata(tmpcfg, datain);
% restore the provenance information
[cfg, datain] = rollback_provenance(cfg, datain);

% match the input data's channels with the labels in the leadfield
sourcemodel = cfg.sourcemodel;
if ~isfield(sourcemodel, 'leadfield')
  ft_error('cfg.sourcemodel needs to contain leadfields');
end
[indx1, indx2] = match_str(datain.label, sourcemodel.label);
if ~isequal(indx1(:),(1:numel(datain.label))')
  ft_error('unsupported mismatch between data channels and leadfields');
end
if islogical(sourcemodel.inside)
  inside = find(sourcemodel.inside);
else
  inside = sourcemodel.inside;
end
for k = inside(:)'
  sourcemodel.leadfield{k} = sourcemodel.leadfield{k}(indx2,:);
end

% compute the Gram-matrix of the supplied forward model
lf = cat(2, sourcemodel.leadfield{:});
G  = lf*lf';

dat     = cat(2,datain.trial{:});
[dum, Ae, N, Nspace, Sout, Sin, Sspace, S] = dssp(dat, G, cfg.dssp.n_in, cfg.dssp.n_out, cfg.dssp.n_space, cfg.dssp.n_intersect);
datAe   = dat*Ae; % the projection is a right multiplication
% with a matrix (eye(size(Ae,1))-Ae*Ae'), since Ae*Ae' can become quite
% sizeable, it's computed slightly differently here.

% put some diagnostic information in the output cfg.
cfg.dssp.S_space        = Sspace;
cfg.dssp.n_space        = Nspace;
cfg.dssp.S_out          = Sout;
cfg.dssp.S_in           = Sin;
cfg.dssp.S_intersect    = S;
cfg.dssp.n_intersect    = N;

% compute the cleaned data and put in a cell-array
nsmp  = cellfun(@numel, datain.time);
csmp  = cumsum([0 nsmp]);
trial = cell(size(datain.trial));
switch cfg.output
  case 'original'
    for k = 1:numel(datain.trial)
      trial{k} = datain.trial{k} - datAe*Ae((csmp(k)+1):csmp(k+1),:)';
    end
  case 'complement'
    for k = 1:numel(datain.trial)
      trial{k} = datAe*Ae((csmp(k)+1):csmp(k+1),:)';
    end
  otherwise
    ft_error(sprintf('cfg.output = ''%s'' is not implemented',cfg.output));
end

% create the output argument
dataout       = keepfields(datain, {'label','time','fsample','trialinfo','sampleinfo','grad', 'elec', 'opto'}); % grad can be kept and does not need to be balanced, since the cleaned data is a mixture over time, not space.
dataout.trial = trial;

ft_postamble debug
ft_postamble trackconfig
ft_postamble previous   datain
ft_postamble provenance dataout
ft_postamble history    dataout
ft_postamble savevar    dataout

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions for the computation of the projection matrix
% kindly provided by Kensuke, and adjusted a bit by Jan-Mathijs
function [Bclean, Ae, Nee, Nspace, Sout, Sin, Sspace, S] = dssp(B, G, Nin, Nout, Nspace, Nee)

% Nc: number of sensors
% Nt: number of time points
% inputs
% B(Nc,Nt):  interference overlapped sensor data
% G(Nc,Nc): Gram matrix of voxel lead field
% Nout and Nin: dimensions of the two row spaces
% recom_Nspace: recommended value for the dimension of the pseudo-signal subspace
% outputs
% Bclean(Nc,Nt): cleaned sensor data
% Nee: dimension of the intersection
% Nspace: dimension of the pseudo-signal subspace
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%
% The code below is modified by Jan-Mathijs, no functional changes
% merely cosmetics

% eigen decomposition of the Gram matrix, matrix describing the spatial
% components
[U,S]   = eig(G);
Sspace  = abs(diag(S));

[Sspace, iorder] = sort(-Sspace);
Sspace           = -Sspace;
U(:,:)           = U(:,iorder);

if isempty(Nspace)
  ttext = 'enter the spatial dimension: ';
  Nspace    = input(ttext);
elseif ischar(Nspace) && isequal(Nspace, 'interactive')
  figure, plot(log10(Sspace),'-o');
  Nspace = input('enter spatial dimension of the ROI subspace: ');
elseif ischar(Nspace) && isequal(Nspace, 'all')
  Nspace = find(Sspace./Sspace(1)>1e5*eps, 1, 'last');
end
fprintf('Using %d spatial dimensions\n', Nspace);

% spatial subspace projector
Us   = U(:,1:Nspace);
USUS = Us*Us';

% Bin and Bout creations
Bin  =                  USUS  * B;
Bout = (eye(size(USUS))-USUS) * B;

% create the temporal subspace projector and apply it to the data
%[AeAe, Nee] = CSP01(Bin, Bout, Nout, Nin, Nee);
%Bclean      = B*(eye(size(AeAe))-AeAe);

[Ae, Nee, Sout, Sin, S] = CSP01(Bin, Bout, Nin, Nout, Nee);
Bclean    = B - (B*Ae)*Ae'; % avoid computation of Ae*Ae'

function [Ae, Nee, Sout, Sin, S] = CSP01(Bin, Bout, Nin, Nout, Nee)
%
% interference rejection by removing the common temporal subspace of the two subspaces
% K. Sekihara,  March 28, 2012
% Golub and Van Loan, Matrix computations, The Johns Hopkins University Press, 1996
%
%  Nc: number of channels
%  Nt: number of time points
% inputs
%  Bout(1:Nc,1:Nt): interference data
%  Bin(1:Nc,1:Nt): signal plus interference data
%  ypost(1:Nc,1:Nt): denoised data
%  Nout: dimension of the interference subspace
%  Nin: dimension of the signal plus interference subspace
%  Nee: dimension of the intersection of the two subspaces
% outputs
% Ae = matrix from which the projector onto the intersection can
%      be obtained:
% AeAe: projector onto the intersection, which is equal to the
%       interference subspace.
% Nee: dimension of the intersection
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%

[dum,Sout,Vout] = svd(Bout,'econ');
[dum,Sin, Vin]  = svd(Bin, 'econ');
Sout = diag(Sout);
Sin  = diag(Sin);

if isempty(Nout)
  ttext = 'enter the spatial dimension for the outside field: ';
  Nout  = input(ttext);
elseif ischar(Nout) && isequal(Nout, 'interactive')
  figure, plot(Sout,'-o');
  Nout = input('enter dimension of the outside field: ');
elseif ischar(Nout) && isequal(Nout, 'all')
  Nout = find(Sout./Sout(1)>1e5*eps, 1, 'last');
end
fprintf('Using %d spatial dimensions for the outside field\n', Nout);

if isempty(Nin)
  ttext = 'enter the spatial dimension for the inside field: ';
  Nin  = input(ttext);
elseif ischar(Nin) && isequal(Nin, 'interactive')
  figure, plot(log10(Sin),'-o');
  Nin = input('enter dimension of the inside field: ');
elseif ischar(Nin) && isequal(Nin, 'all')
  Nin = find(Sin./Sin(1)>1e5*eps, 1, 'last');
end
fprintf('Using %d spatial dimensions for the inside field\n', Nin);

Qout = Vout(:,1:Nout);
Qin  = Vin(:, 1:Nin);

C     = Qin'*Qout;
[U,S] = svd(C);
S     = diag(S);
if (ischar(Nee) && strcmp(Nee, 'auto'))
  ft_error('automatic determination of intersection dimension is not supported');
elseif ischar(Nee) && strcmp(Nee, 'interactive')
  figure, plot(S,'-o');
  Nee  = input('enter dimension of the intersection: ');
elseif Nee<1
  % treat a numeric value < 1 as a threshold
  Nee = find(S>Nee,1,'last');
  if isempty(Nee), Nee = 1; end
end
fprintf('Using %d dimensions for the interaction\n', Nee);

Ae   = Qin*U;
Ae   = Ae(:,1:Nee);
%AeAe = Ae*Ae';