function varargout = vl_nnmax(M, varargin)
% VL_NNMAX Element-wise maximum
%   Y = VL_NNMAX(M, X1, ..., XM) applies the elementwise max operator to
%   the given M input tensors, each of which should have the same input
%   dimension.
%
%   [DZDX1,..,DZDXM] = VL_NNMAX(M,X1, ...,XM, DZDY) computes the
%   derivatives of the block projected onto DZDY.
%
% Copyright (C) 2017 Samuel Albanie
% Licensed under The MIT License [see LICENSE.md for details]

  assert(M >= 2, 'at least two input tensors expected') ;
  ins = varargin(1:M) ; varargin(1:M) = [] ;
  [~, dzdy] = vl_argparsepos(struct(), varargin) ;

  shared = cat(5, ins{:}) ;
  if isempty(dzdy)
     varargout{1} = max(shared, [], 5) ;
  else
    % recompute forward max and insert derivatives at max locations
    [~,I] = max(shared, [], 5) ;
    dzdx = zeros(size(shared), 'like', shared) ;
    offsets = (I - 1) .* numel(dzdy{1}) ;
    idx = offsets(:) + (1:numel(dzdy{1}))' ;
    dzdx(idx) = dzdy{1} ;
    varargout = arrayfun(@(x) {dzdx(:,:,:,:,x)}, 1:M) ;
  end


function Matrices = removeExplorationConstraints(Matrices);
candidates = find((~any(Matrices.G,2)) & (sum(Matrices.E | Matrices.E,2) == 1));
if ~isempty(candidates)
    Matrices.bndA = -Matrices.E(candidates,:);
    Matrices.bndb = Matrices.W(candidates,:);
    Matrices.G(candidates,:) = [];
    Matrices.E(candidates,:) = [];
    Matrices.W(candidates,:) = [];
end


function sF = norm(sVF)
% pointwise norm of the vectorfield
%
% Syntax
%   norm(sVF)
%
% Output
%   sF - S2FunTri
%

sF = S2FunTri(sVF.tri,norm(sVF.values));

end


classdef computingOptimal < handle

    methods (Access = public)

        function obj = computingOptimal()
            obj.init()
        end

    end

    methods (Access = private)

        function init(obj)

        end

        function obj = computingOpt()
            fileName = 'OptimalSuperEllipse';
            outputFolder = fullfile(pwd,'Output',fileName);
            gmsFile = [fullfile(outputFolder,fileName),'.msh'];
            createMesh(fileName,outputFolder);
            homog = obj.createNumericalHomogenizer(fileName,gmsFile);
            maxSnorm = obj.computeMaxStressNorm(homog);
        end

        function maxSnorm = computeMaxStressNorm(obj,homog)
            Ch = homog.cellVariables.Ch;
            microProblem = homog.getMicroProblem();
            stress = [cos(pi/4) sin(pi/4) 0]';
            strain = Ch\stress;
            microProblem.element.setVstrain(strain');
            microProblem.computeVariables;
            stresses = microProblem.variables.stress;
            sx = squeeze(stresses(:,1,:));
            sy = squeeze(stresses(:,2,:));
            sxy = squeeze(stresses(:,3,:));
            sNorm = sqrt(sx.*sx + 2*sxy.*sxy + sy.*sy);
            maxSnorm = max(sNorm);
        end

        function createMesh(obj,fileName,outputFolder)
            d = SettingsFreeFemMeshGenerator();
            d.freeFemFileName = 'SmoothRectangle';
            d.hMax  = 0.02;%0.0025;
            d.mxV             = 0.5;
            d.myV             = 0.5;
            d.fileName        = fileName;
            d.printingDir     = outputFolder;
            d.qNorm           = 32;

            fG = FreeFemMeshGenerator(d);
            fG.generate();

        end

        function homog = createNumericalHomogenizer(obj,fileName,gmsFile)
            d.gmsFile = gmsFile;
            d.outFile = fileName;
            d.print   = false;
            d.iter = 0;
            nH = NumericalHomogenizerCreatorFromGmsFile(d);
            homog = nH.getHomogenizer();
        end

    end

end


function arrayIndices = sample2index(this, samplingPoints, iDims)
% Returns voxel samplingPoints corresponding to label (coordinate) samplingPoints
%
%   Y = MrDimInfo()
%   arrayIndices = sample2index(this, samplingPoints)
%
% Inverse operation to get_samplingPoints
%
% This is a method of class MrDimInfo.
%
% IN
%   samplingPoints     [nVoxels, nDims] of voxel samplingPoints
%                      (one per row) in coordinate system given by dimInfo
%   iDims              if specified, samplingPoints are assumed to be subset
%                      of all dimensions only
%
% OUT
%   arrayIndices        [nVoxels, nDims] of absolute
%                       voxel samplingPoints within array
%
% EXAMPLE
%   sample2index
%
%   See also MrDimInfo MrDimInfo.get_samplingPoints

% Author:   Saskia Bollmann & Lars Kasper
% Created:  2016-01-23
% Copyright (C) 2016 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

if nargin < 3
    iDims = 1:this.nDims;
end

nDimsSubset = numel(iDims);

isRowVector = nDimsSubset == 1 && size(samplingPoints,1) == 1;

if isRowVector
   samplingPoints = samplingPoints(:); % allows row/column vectors as input for 1-dim trafo
end

nVoxels = size(samplingPoints,1);

arrayIndices = zeros(nVoxels,nDimsSubset);

for v = 1:nVoxels
    % find voxel index with closest (euclidean) sampling point in array
    for d = 1:nDimsSubset
        iDim = iDims(d);
        [~, arrayIndices(v,d)] = ...
            min(abs(this.samplingPoints{iDim} - samplingPoints(v,d)));
    end
end

if isRowVector % transform back to row vector for output
    arrayIndices = reshape(arrayIndices, 1, []);
end


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the resting lengths and/or spring stiffness
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs = update_Springs(dt,current_time,xLag,yLag,springs)

% dt: time-step
% current_time: current time of simulation
% xLag: Vector of all xLagrangian Pts
% springs: col 1: starting spring pt (by lag. discretization)
%          col 2: ending spring pt. (by lag. discretization)
%          col 3: spring stiffness
%          col 4: spring resting lengths

s_1 = springs(:,1);    % MASTER Node Indices
s_2 = springs(:,2);    % SLAVE Node Indices
k_Vec = springs(:,3);  % Springs Stiffnesses Vector
RL_Vec= springs(:,4);  % Resting Lengths Vectors

N = length(xLag);                   % Gives total number of Lagrangian pts!
N_springs = length( springs(:,1) ); % Gives total number of springs!

freq = 10;  % Frequency for pumping
d = 1.0;    % Diameter of Heart Tube

for i=(N-2)+10:(N-2)+20            % Loops over desired springs!

    RL_Vec(i) = d - abs( 0.9*d*sin( 2*pi*freq*current_time ) );

end

springs(:,4) = RL_Vec; % Update the springs_info


function rtk=detslip_tdcpins(rtk,cur_obs,old_obs,nav,rcv)

global glc
dt=timediff(cur_obs(1).time,old_obs(1).time);
if abs(dt)>5
    return;
end

cur_sv=satposs(cur_obs,nav,rtk.opt.sateph);
old_sv=satposs(old_obs,nav,rtk.opt.sateph);

if rcv==1
    cur_pos=rtk.ins.pos;     cur_rr=blh2xyz(cur_pos);
    old_pos=rtk.ins.oldpos;  old_rr=blh2xyz(old_pos);
elseif rcv==2
    cur_rr=rtk.basepos;  cur_pos=xyz2blh(cur_rr);
    old_rr=cur_rr;       old_pos=cur_pos;
end

n_cur=size(cur_obs,1);
n_old=size(old_obs,1);

v=zeros(n_cur,1); H=zeros(n_cur,4); P=zeros(n_cur,n_cur);
nv=0; vsat=zeros(n_cur,1); vv=zeros(n_cur,1);

for i=1:n_cur

    cur_obsi=cur_obs(i); cur_svi=cur_sv(i);
    lam=nav.lam(cur_obsi.sat,:);
    [sys,~]=satsys(cur_obsi.sat);
    if rtk.mask(sys)==0,continue;end

    if lam(1)==0,continue;end

    old_idx=0;
    for j=1:n_old
        if cur_obsi.sat==old_obs(j).sat
            old_idx=j;break;
        end
    end
    if old_idx==0,continue;end

    old_obsi=old_obs(old_idx); old_svi=old_sv(old_idx);

    if cur_svi.svh~=0||old_svi.svh~=0||norm(cur_svi.pos)<=0||norm(old_svi.pos)<=0
        continue;
    end
    if cur_obsi.L(1)==0||old_obsi.L(1)==0,continue;end

    [cur_range,cur_LOS]=geodist(cur_svi.pos,cur_rr);
    cur_azel=satazel(cur_pos,cur_LOS);
    [old_range,old_LOS]=geodist(old_svi.pos,old_rr);
    old_azel=satazel(old_pos,old_LOS); %#ok

    tdcp_obs=cur_obsi.L(1)*lam(1)-old_obsi.L(1)*lam(1);
    tdcp_pre=cur_range-old_range;

    v(nv+1,1)=tdcp_obs-tdcp_pre+glc.CLIGHT*(cur_svi.dts-old_svi.dts);

    H(nv+1,:)=[-cur_LOS,1];

    P(nv+1,nv+1)=(0.05^2/sin(cur_azel(2))^2)^-1;

    vv(i,1)=v(nv+1,1);

    nv=nv+1;

    vsat(i)=1;

end

if nv<4,return;end
if nv<n_cur
    v(nv+1:end,:)=[]; H(nv+1:end,:)=[]; P(nv+1:end,:)=[];P(:,nv+1:end)=[];
end

nv=size(v,1);exc=zeros(nv,1);
for i=1:nv
    vi=abs(v(i));
    v_other=abs(v);v_other(i)=[];
    ave_v=sum(abs(v_other))/(nv-1);
    ave_distance=sum(abs(vi-v_other))/(nv-1);
    if ave_distance>3*ave_v
        exc(i)=1;
    end
end
n=0; v0=zeros(nv,1); H0=zeros(nv,4); P0=zeros(nv,nv);
for i=1:nv
    if exc(i)==1,continue;end
    v0(n+1,1)=v(i);
    H0(n+1,:)=H(i,:);
    P0(n+1,n+1)=P(i,i);
    n=n+1;
end
if n<4,return;end
if n<nv
    v0(n+1:end,:)=[]; H0(n+1:end,:)=[]; P0(n+1:end,:)=[];P0(:,n+1:end)=[];
end

[dx,~]=least_square(v0,H0,P0);
vv=vv-dx(4);

slip=zeros(n_cur,1);
for i=1:n_cur
    sat=cur_obs(i).sat;
    if vsat(i)==0
        rtk.sat(sat).slip(1)=bitor(rtk.sat(sat).slip(1),1);
        continue;
    end

    vi=abs(vv(i));
    v_other=abs(vv);v_other(i)=[];
    ave_v=sum(abs(v_other))/(nv-1);
    ave_distance=sum(abs(vi-v_other))/(nv-1);
    if ave_distance>3*ave_v&&abs(vv(i))>0.15
        slip(i)=1;
    end
end

idx=find(slip==1);
ratio=size(idx,1)/n_cur;
if ratio>=0.5
    return;
end

for i=1:n_cur
    sat=cur_obs(i).sat;
    if slip(i)==1
        rtk.sat(sat).slip(1)=bitor(rtk.sat(sat).slip(1),slip(i));
    end
end

return


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 output = callclp(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
ub      = interfacedata.ub;
lb      = interfacedata.lb;
Q       = interfacedata.Q;

n = length(c);

if options.showprogress;showprogress(['Calling CLP'],options.showprogress);end

if K.f>0
    Aeq = -F_struc(1:K.f,2:end);
    beq = F_struc(1:K.f,1);
else
    Aeq = [];
    beq = [];
end

if any(K.l)
    A = -F_struc(1+K.f:end,2:end);
    b = F_struc(1+K.f:end,1);
else
    A = [];
    b = [];
end

% Fix for bug in mexclp or clp
fixed = 0;
if size(A,1)==0 & size(Aeq,2)==0
    A = [ones(1,length(c))];
    if ~isempty(ub)
        A(isinf(ub)) = 0;
        dummy = ub;
        dummy(isinf(ub)) = 0;
        b = sum(dummy);
    else
        b = 1e8;
    end
    fixed = 1;
    options.saveduals = 0;
end
% lb(lb==-inf)=-1e12
% ub(ub==inf)=1e12

ops = options.clp;
ops.verbose = options.verbose;

if options.savedebug
    save clpdebug
end

% Call mex-interfacec
solvertime = tic;
[x,lambda,problem] = clp(2*Q,c,A,b,Aeq,beq,lb,ub,ops);%,interfacedata.integer_variables);
solvertime = toc(solvertime);

if options.saveduals
    D_struc = -lambda;
else
    D_struc = [];
end

% Save all data sent to solver?
if options.savesolverinput
	solverinput = [];
else
	solverinput = [];
end

% Save all data from the solver?
if options.savesolveroutput
	solveroutput = [];
else
	solveroutput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);


function mec_weighting = lmsSR_computeWeightsMECv4(mec_confMapN, mec_thr_scaler, mec_weight_exp)
%
% LMSSR_COMPUTEWEIGHTSMECV4 Computes weights depending on the quality of the motion estimation/compensation stage. (Version 4: One global threshold, adapted by number of images)
%    mec_weighting = LMSSR_COMPUTEWEIGHTSMECV4(mec_confMapN, mec_thr_scaler, mec_weight_exp)
%
% Parameters: mec_confMapN     -   3-D matrix containing confidence information in form of SSD values for each pixel and each auxiliary frame
%             mec_thr_scaler   -   Scalar value responsible for scaling the standard deviation threshold which cuts off SSD values that are too large [default: 20]
%             mec_weight_exp   -   Exponent for intensifying the weighting [default: 2]
%
%
% Author: Michel Bätz (LMS)
%
% See also: lmsSR_framework
%

[height,width,num_imgs] = size(mec_confMapN);

% 1-SSD Weighting + Setting SSD values larger than twice the standard deviation to 0
mec_weighting   = ones(height,width,num_imgs+1); % Highest weight is set to 1 => Reference frame weights need no further adaptation
mec_conf_stddev = std(mec_confMapN(:));
mec_weighting(:,:,2:end) = 1 - (mec_confMapN / (mec_thr_scaler*(1/num_imgs)*mec_conf_stddev)); % SSD values larger than threshold_scaler times standard deviation are set to 0 [default: 20]
mec_weighting(mec_weighting < 0) = 0;

%mec_weighting(mec_weighting > 0) = 1; % QUICK-TEST
mec_weighting = mec_weighting.^mec_weight_exp; % Default: 2


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
function amgausst
%AMGAUSST Unit test for the function AMGAUSS.

%	O. Lemoine - February 1996.

N=256; t0=149; T=50;
sig=amgauss(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amgauss test 1 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>sqrt(eps),					% width
 error('amgauss test 2 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amgauss test 3 failed ');
end

N=121; t0=60; T=27;
sig=amgauss(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amgauss test 4 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>sqrt(eps),					% width
 error('amgauss test 5 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amgauss test 6 failed ');
end

N=534; t0=333; T=70;
sig=amgauss(N,t0,T);
if abs(sig(t0)-1)>sqrt(eps),				% sig(t0)=1
 error('amgauss test 7 failed ');
end
[tm,T1]=loctime(sig);
if abs(T-T1)>sqrt(eps),					% width
 error('amgauss test 8 failed ');
end
dist=1:min([N-t0,t0-1]);
if any(abs(sig(t0-dist)-sig(t0+dist))>sqrt(eps))~=0, 	% symmetry
 error('amgauss test 9 failed ');
end


function S = querylpv(lpv, p)
%QUERYLPV Query an LPV model at given parameter point
%	S = QUERYLPV(lpv, p)
%
%	lpv   - LPV model
%	p     - given parameter
%
%	S     - system matrix at the given point

% TODO: convert to lti sys

S = zeros(size(lpv));
for i = 1:size(lpv,1)
	for j = 1:size(lpv,2)
		S(i,j) = lpv{i,j}(p);
	end
end


function t = subsasgn(t,s,b)
%SUBSASGN Subscripted assignement for ktensor.
%
%   See also KTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

switch s(1).type
    case '.'
        switch s(1).subs
            case 'lambda'
                if length(s) == 1
                    t = ktensor(b, t.u);
                else
                    newlambda = subsasgn(t.lambda, s(2:end), b);
                    t = ktensor(newlambda, t.u);
                end
            case {'u','U'}
                if length(s) == 1
                    t = ktensor(t.lambda, b);
                else
                    tmpu = subsasgn(t.u, s(2:end), b);
                    t = ktensor(t.lambda, tmpu);
                end
            otherwise
                error(['No such field: ', s(1).subs]);
        end
    case '()'
        error('Cannot change individual entries in a ktensor.')
    case '{}'
        new_s(1).type = '.';
        new_s(1).subs = 'u';
        new_s(2:length(s)+1) = s;
        t = subsasgn(t, new_s, b);
    otherwise
        error('Invalid subsasgn.');
end


function displayIm = shownsct( y )
% SHOWNSSC   Show nonsubsampled Contourlet transform coefficients.
%
%       shownsct(y)
% Input:
%	y:	a cell vector of length n+1, one for each layer of
%		subband images from NSCT, y{1} is the lowpass image
%
% NOTE:
%       It need further improvement later!!!!

% History:
%   08/08/2003  Created by Jianping Zhou.

% Level of decomposition.
clevels = length( y ) ;

% Show the subband images.
for i=1:clevels
    figure;
    if iscell( y{i} )
        % The number of directional subbands.
        csubband = length( y{i} ) ;
        if csubband > 7
            col = 4 ;
        else
            col = 2 ;
        end
        row = csubband / col ;
        for j = 1:csubband
            subplot( row, col, j ) ;
            imshow( uint8(y{i}{j}) );
            title( sprintf('NSSC coefficients: level %d', i) );
        end
    else
        imshow ( uint8(y{i}) ) ;
        title( sprintf('Nonsubsampled Contourlet coefficients level %d', i) );
    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


function r4_uniform_01_test ( )

%*****************************************************************************80
%
%% R4_UNIFORM_01_TEST tests R4_UNIFORM_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R4_UNIFORM_01_TEST\n' );
  fprintf ( 1, '  R4_UNIFORM_01 produces a sequence of random values.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  SEED = %d\n', seed );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ r, seed ] = r4_uniform_01 ( seed );
    fprintf ( 1, '  %2d  %14g\n', i, r );
  end

  return
end


function deg = degree(x,y,z)
%DEGREE Polynomial degree
%
% DEG = DEGREE(p,x,flag,vector)
%
% p      : SDPVAR object.
% x      : Degree w.r.t linear SDPVAR objects.
% flag   : 'max', 'min'. Default 'max'
% vector : If vector = 1, returns degree of each element in p
%
% Examples
% x1 = sdpvar(1,1);x2 = sdpvar(1,1);
% p = [x1;x1*x2+x2^2];
%
% degree(p) returns 2
%
% degree(p,x1) returns 1
%
% degree(p,[x1 x2]) returns [1 2]
%
% degree(p,[x1 x2],'max',1) returns [1 0;1 2]
%
% degree(p,[],1) returns [1;2]

if isa(x,'double')
    deg = 0;
else
    error('DEGREE only defined for SDPVAR and DOUBLE.');
end


function plot_catalog(mCatalog, vMain,bFaults)

if bFaults
    load ~/data/faults/mCA-faults.mat
end
figure
plot(mCatalog(:,1),mCatalog(:,2),'k.');
hold on;plot(mCatalog(~vMain,1),mCatalog(~vMain,2),'r.');
xlim([-118.2 -115]);
ylim([32 36]);
set(gca,'FontSize',16);
if bFaults
    hold on; plot(mFaults(:,1),mFaults(:,2),'k-','LineWidth',1);
end

figure
plot(mCatalog(:,3),cumsum(ones(size(mCatalog,1),1)),'r',...
    'LineWidth',2);
hold on;plot(mCatalog(:,3),cumsum(vMain),'k',...
    'LineWidth',2);
set(gca,'FontSize',16);


function []=panel4fevddisp(n,N,Units,endo,fevd_estimates,IRFperiods,pref)

if pref.plot
    % plot the figure
    fevd=figure('Tag','BEARresults');
    set(fevd,'Color',[0.9 0.9 0.9]);
    set(fevd,'name','forecast error variance decomposition');
    % initiate the count
    count=0;
    % loop over units
    for ii=1:N
        % loop over endogenous variables
        for jj=1:n
            % loop over shocks
            for kk=1:n
                % increment count
                count=count+1;
                % then plot
                subplot(N*n,n,count)
                temp=fevd_estimates{jj,kk,ii};
                hold on
                Xpatch=[(1:IRFperiods) (IRFperiods:-1:1)];
                Ypatch=[temp(1,:) fliplr(temp(3,:))];
                FEVDpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
                set(FEVDpatch,'facealpha',0.5);
                set(FEVDpatch,'edgecolor','none');
                plot(temp(2,:),'Color',[0.4 0.4 1],'LineWidth',2);
                hold off
                set(gca,'XLim',[1 IRFperiods],'YLim',[0 1],'FontName','Times New Roman');
                % top labels
                if jj==1
                    title([Units{ii,1} '\_' endo{kk,1}],'FontWeight','normal');
                end
                % side labels
                if kk==1
                    ylabel([Units{ii,1} '\_' endo{jj,1}],'FontWeight','normal');
                end
            end
        end
    end
    % top supertitle
    ax=axes('Units','Normal','Position',[.11 .075 .85 .88],'Visible','off');
    set(get(ax,'Title'),'Visible','on')
    title('Contribution of shock:','FontSize',11,'FontName','Times New Roman','FontWeight','normal');
    % side supertitle
    ylabel('Variance of:','FontSize',12,'FontName','Times New Roman','FontWeight','normal');
    set(get(ax,'Ylabel'),'Visible','on');
end

% save on Excel
% create the cell that will be saved on excel
fevdcell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},IRFperiods+3,1);
horzspace=repmat({''},2,5*n);
% loop over units
for ii=1:N
    % loop over endogenous variables
    for jj=1:n
        % initiate the cell of results
        endocell={};
        % loop over shocks
        for kk=1:n
            % create a header
            header=[{['part of ' Units{ii,1} '_' endo{jj,1} ' fluctuation due to ' Units{ii,1} '_' endo{kk,1} ' shocks']} {''} {''} {''};{''} {''} {''} {''};{''} {'lower bound'} {'median'} {'upper bound'}];
            % complete the cell
            tempcell=[[header;num2cell((1:IRFperiods)') num2cell((fevd_estimates{jj,kk,ii})')] vertspace];
            % concatenate to the previous parts of unitcell
            endocell=[endocell tempcell];
        end
        % concatenate to the previous parts of irfcell
        fevdcell=[fevdcell;horzspace;endocell];
    end
    fevdcell=[fevdcell;horzspace];
end
% trim
fevdcell=fevdcell(3:end-2,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),fevdcell,'FEVD','B2');
end


function [ y, symm ] = cvx_s_banded( m, n, symm, lower, upper )

%CVX_S_BANDED (U,L)-banded matrices.

if nargin < 4,
    error( 'Bandwidth arguments missing.' );
end
if ~isnumeric( lower ) || length( lower ) ~= 1 || lower < 0 || lower ~= floor( lower ),
    error( 'Bandwidth arguments must be nonnegative integers.' );
elseif nargin < 5,
    upper = lower;
elseif ~isnumeric( upper ) || length( upper ) ~= 1 || upper < 0 || upper ~= floor( upper ),
    error( 'Bandwidth arguments must be nonnegative integers.' );
end

stflag = length(symm) == 2;
if stflag,
    toep = symm(2);
    symm = symm(1);
else
    toep = false;
end

if symm,
    lower = min( lower, upper );
    upper = 0;
end

c    = 0 : n - 1;
c    = c( ones( 1, m ), : );
r    = ( 0 : m - 1 )';
r    = r( :, ones( 1, n ) );
temp = r - c;
temp = temp <= lower & temp >= -upper;
r    = r( temp );
c    = c( temp );
nu   = length( r );

if toep,
    v = r - c;
    v = abs( v ) + max( v ) * ( v < 0 ) + 1;
    nu = max( v );
    toep = false;
else
    v = ( 1 : nu )';
end

if symm,
    tt = r ~= c;
    r = [ r ; c(tt) ];
    c = [ c ; r(tt) ];
    v = [ v ; v(tt) ];
    symm = false;
end

if stflag,
    symm = [ symm, toep ];
end

y = sparse( v, r + m * c + 1, 1, nu, m * n );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


filename = 'ArchTriFine';%'CantileverSquareNewFine';%'LshapeTriFine';%'CantileverSquareNew';'ArchTriFine';%'CantileverSquare';%'ArchTriFine';%'CantileverSquare';%'Lshape';%'LshapeTriFine';%'CantileverSquareSmall';%'';%'Lshape';'CantileverSquare';'Lshape';%'CantileverSquare';%'LshapeFine';%'Bridge_quad_coarse';%'Arch_quad_coarse';%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';%'Bridge';%'CantileverSquareSmall';
ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'PDE';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 64;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
%vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 50;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 1000;


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


% rigid registration of frames with offsets ds
function [dreg] = rigidMovie(data, ops1, dsall, yFOVs, xFOVs)

ix0 = 0;
Nbatch = 1000;
dreg = zeros(size(data), 'single');
while ix0<size(data,3)
    indxr = ix0 + (1:Nbatch);
    indxr(indxr>size(data,3)) = [];
    for l = 1:size(xFOVs,2)
        dreg(yFOVs(:,l), xFOVs(:,l), indxr)        = ...
            rigidRegFrames(data(yFOVs(:,l), xFOVs(:,l), indxr), ops1{1,l}, dsall(indxr,:,l));
    end
    ix0 = ix0 + Nbatch;
end


function deformed_image = PTKInterpolateToMatch(original_image, deformation_field, interpolation_type, reporting)
    % PTKInterpolateToMatch. Interpolates an image to match the origin and voxel
    %     size specified in a deformation field
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    % This is similar to PTKDeformImage but there is no actual image deformation;
    % we are just reinterpolating to match the template provided by the
    % deformation field
    deformed_image = deformation_field.BlankCopy;
    deformed_image.ImageType = PTKImageType.Grayscale;
    [i_o, j_o, k_o] = original_image.GetGlobalCoordinatesMm;
    [i_o, j_o, k_o] = ndgrid(i_o, j_o, k_o);

    [i_r, j_r, k_r] = deformation_field.GetGlobalCoordinatesMm;
    [i_r, j_r, k_r] = ndgrid(i_r, j_r, k_r);

   deformed_image_raw = interpn(i_o, j_o, k_o, single(original_image.RawImage), ...
       i_r , ...
       j_r , ...
       k_r , ...
       interpolation_type, 0);
   deformed_image.ChangeRawImage(deformed_image_raw);
end


function epoch = mydatesowi (sow, epoch2)
    bow2 = mydatesow_aux (epoch2);
    epoch = sow + bow2;
end

%!test
%! % on a Sunday, sow == sod.
%! vec = [2008 11 30  13 14 15];
%! num = mydatenum(vec);
%! sod = mydatesod(num);
%! sow = mydatesow(num);
%! num2 = mydatesodi(sod, num);
%! num3 = mydatesowi(sow, num);
%! %sod, sow  % DEBUG
%! %num, num2, num3  % DEBUG
%! myassert (sow, sod, -sqrt(eps));
%! myassert (num2, num, -sqrt(eps));
%! myassert (num3, num, -sqrt(eps));

%!test
%! % random sow (within one weeek):
%! sow = randint(0, 7*24*3600);
%! bow_vec = [2008 11 30  0 0 0];
%! bow_num = mydatenum(bow_vec);
%! num = bow_num + sow;
%! sow2 = mydatesow(num);
%! num2 = mydatesowi(sow2, num);
%! %sow2, sow, sow2-sow  % DEBUG
%! %num2, num, num2-num  % DEBUG
%! tol = 1e-7;
%! myassert (sow2, sow, -tol);
%! myassert (num2, num, -tol);

%!test
%! % epoch2 <> epoch.
%! sow = randint(0, 7*24*3600);
%! bow_vec = [2008 11 30  0 0 0];
%! bow_num = mydatenum(bow_vec);
%! num = bow_num + sow;
%! bow_vec2 = [2008 11 30-7  0 0 0];
%! bow_num2 = mydatenum(bow_vec2);
%! sow2a = mydatesow(num, bow_num);
%! sow2b = mydatesow(num, bow_num2);
%! num2a = mydatesowi(sow2a, bow_num);
%! num2b = mydatesowi(sow2b, bow_num2);
%! %num, num2a, num2b  % DEBUG
%! tol = 1e-7;
%! myassert (sow2a, sow, -tol);
%! myassert (sow2b, sow+7*24*3600, -tol);
%! myassert (num2a, num, -tol);
%! myassert (num2b, num, -tol);


function tap_delay = tapped_delay_fcn(input)
% The Tapped Delay function delays its input by the specified number
% of sample periods, and outputs all the delayed versions in a vector
% form. The output includes current input

% NOTE: To instruct Embedded MATLAB to compile an external function,
% add the following compilation directive or pragma to the function code
%#eml

persistent u_d;
if isempty(u_d)
    u_d = fi(zeros(1,40), numerictype(input), fimath(input));
end

u_d = [u_d(2:40), input];

tap_delay = u_d;


function  varargout = drawPlatform(plane, siz, varargin)
%DRAWPLATFORM Draw a rectangular platform with a given size.
%
%   drawPlatform(PLANE, SIZ) draws a rectangular platform with the
%   dimensions specified by SIZ. If SIZ contains only one value instead of
%   two the platform will be quadratic.
%
%   drawPlatform(...,'PropertyName',PropertyValue,...) sets the value of
%   the specified patch property. Multiple property values can be set with
%   a single statement. See function patch for details.
%
%   drawPlane3d(AX,...) plots into AX instead of GCA.
%
%   H = drawPlatform(...) returns a handle H to the patch object.
%
%   Example
%
%     p0 = [1 2 3];
%     v1 = [1 0 1];
%     v2 = [0 -1 1];
%     plane = [p0 v1 v2];
%     axis([-10 10 -10 10 -10 10]);
%     drawPlatform(plane, [7,3])
%     set(gcf, 'renderer', 'zbuffer');
%
%   See also
%   planes3d, createPlane, patch

% ------
% Author: oqilipo
% Created: 2018-08-09
% Copyright 2018

%% Parse inputs

% extract axis handle
if numel(plane) == 1 && ishandle(plane)
    hAx = plane;
    plane = siz;
    siz = varargin{1};
    varargin(1) = [];
else
    hAx = gca;
end

% parse optional arguments
p = inputParser;
addRequired(p, 'plane', @(x) size(x,1)==1 && isPlane(x))
addRequired(p, 'siz', @(x)validateattributes(x,{'numeric'},...
    {'size',[1, nan],'positive','nonnan','real','finite'}))
parse(p, plane, siz)

if ~isempty(varargin)
    if length(varargin) == 1
        if isstruct(varargin{1})
            % if options are specified as struct, need to convert to
            % parameter name-value pairs
            varargin = [fieldnames(varargin{1}) struct2cell(varargin{1})]';
            varargin = varargin(:)';
        else
            % if option is a single argument, assume it corresponds to
            % plane color
            varargin = {'FaceColor', varargin{1}};
        end
    end
else
    % default face color
    varargin = {'FaceColor', 'm'};
end

if numel(siz) == 1
    siz(2) = siz(1);
end

%% Algorithm
% Calculate vertex points of the platform
pts(1,:) = planePoint(plane, [1,1]*0.5.*siz);
pts(2,:) = planePoint(plane, [1,-1]*0.5.*siz);
pts(3,:) = planePoint(plane, [-1,-1]*0.5.*siz);
pts(4,:) = planePoint(plane, [-1,1]*0.5.*siz);

pf.vertices = pts;
pf.faces = [1 2 3 4];

% Draw the patch
h = patch(hAx, pf, varargin{:});

%% Parse outputs
% Return handle to plane if needed
if nargout > 0
    varargout{1} = h;
end

end


function fiff_write_complex_matrix(fid,kind,mat)
%
% fiff_write_complex_matrix(fid,kind,mat)
%
% Writes a single-precision complex matrix tag
%
%     fid           An open fif file descriptor
%     kind          The tag kind
%     mat           The data matrix
%

%
%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.1  2006/09/23 14:43:56  msh
%   Added routines for writing complex and double complex matrices.
%   Added routine for writing double-precision real matrix.
%
%

me='MNE:fiff_write_complex_matrix';

if nargin ~= 3
        error(me,'Incorrect number of arguments');
end

FIFFT_COMPLEX_FLOAT = 20;
FIFFT_MATRIX  = bitshift(1,30);
FIFFT_MATRIX_COMPLEX = bitor(FIFFT_COMPLEX_FLOAT,FIFFT_MATRIX);
FIFFV_NEXT_SEQ=0;

datasize = 2*4*numel(mat) + 4*3;

count = fwrite(fid,int32(kind),'int32');
if count ~= 1
    error(me,'write failed');
end
count = fwrite(fid,int32(FIFFT_MATRIX_COMPLEX),'int32');
if count ~= 1
    error(me,'write failed');
end
count = fwrite(fid,int32(datasize),'int32');
if count ~= 1
    error(me,'write failed');
end
count = fwrite(fid,int32(FIFFV_NEXT_SEQ),'int32');
if count ~= 1
   error(me,'write failed');
end
nrow = size(mat,1);
ncol = size(mat,2);
for j = 1:nrow
   for k = 1:ncol
      count = fwrite(fid,real(mat(j,k)),'single');
      if count ~= 1
          error(me,'write failed');
      end
      count = fwrite(fid,imag(mat(j,k)),'single');
      if count ~= 1
          error(me,'write failed');
      end
   end
end
dims(1) = size(mat,2);
dims(2) = size(mat,1);
dims(3) = 2;
count = fwrite(fid,int32(dims),'int32');
if count ~= 3
    error(me,'write failed');
end

return;


function tests = test_stackRotate90(varargin)
%TEST_STACKROTATE90  One-line description here, please.
%   output = test_stackRotate90(input)
%
%   Example
%   test_stackRotate90
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-05-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

tests = functiontests(localfunctions);

function testRotateX(testCase)

img = createBasicTestImage;

rotX1th = cat(3, [5 6;1 2], [7 8;3 4]);
imgRX1 = stackRotate90(img, 1, 1);
assertEqual(testCase, rotX1th, imgRX1);
imgRX1 = stackRotate90(img, 'x', 1);
assertEqual(testCase, rotX1th, imgRX1);

rotX2th = cat(3, [7 8;5 6], [3 4;1 2]);
imgRX2 = stackRotate90(img, 1, 2);
assertEqual(testCase, rotX2th, imgRX2);
imgRX2 = stackRotate90(img, 'x', 2);
assertEqual(testCase, rotX2th, imgRX2);

rotX3th = cat(3, [3 4;7 8], [1 2;5 6]);
imgRX3 = stackRotate90(img, 1, 3);
assertEqual(testCase, rotX3th, imgRX3);
imgRX3 = stackRotate90(img, 'x', 3);
assertEqual(testCase, rotX3th, imgRX3);

function testRotateY(testCase)

img = createBasicTestImage;

rotY1th = cat(3, [2 6;4 8], [1 5;3 7]);
imgRY1 = stackRotate90(img, 2, 1);
assertEqual(testCase, rotY1th, imgRY1);
imgRY1 = stackRotate90(img, 'y', 1);
assertEqual(testCase, rotY1th, imgRY1);

rotY2th = cat(3, [6 5;8 7], [2 1;4 3]);
imgRY2 = stackRotate90(img, 2, 2);
assertEqual(testCase, rotY2th, imgRY2);
imgRY2 = stackRotate90(img, 'y', 2);
assertEqual(testCase, rotY2th, imgRY2);

rotY3th = cat(3, [5 1;7 3], [6 2;8 4]);
imgRY3 = stackRotate90(img, 2, 3);
assertEqual(testCase, rotY3th, imgRY3);
imgRY3 = stackRotate90(img, 'y', 3);
assertEqual(testCase, rotY3th, imgRY3);

function testRotateZ(testCase)

img = createBasicTestImage;

rotZ1th = cat(3, [3 1;4 2], [7 5;8 6]);
imgRZ1 = stackRotate90(img, 3, 1);
assertEqual(testCase, rotZ1th, imgRZ1);
imgRZ1 = stackRotate90(img, 'z', 1);
assertEqual(testCase, rotZ1th, imgRZ1);

rotZ2th = cat(3, [4 3;2 1], [8 7;6 5]);
imgRZ2 = stackRotate90(img, 3, 2);
assertEqual(testCase, rotZ2th, imgRZ2);
imgRZ2 = stackRotate90(img, 'z', 2);
assertEqual(testCase, rotZ2th, imgRZ2);

rotZ3th = cat(3, [2 4;1 3], [6 8;5 7]);
imgRZ3 = stackRotate90(img, 3, 3);
assertEqual(testCase, rotZ3th, imgRZ3);
imgRZ3 = stackRotate90(img, 'z', 3);
assertEqual(testCase, rotZ3th, imgRZ3);

function testRotateGrayscaleY(testCase)

img = createTestImageGray;
dim = size(img);

% rotate around Y-axis
img2 = stackRotate90(img, 2, 1);

% check dimension
assertEqual(testCase, dim([1 3 2]), size(img2));

function testRotateGrayscaleX(testCase)

img = createTestImageGray;
dim = size(img);

% rotate around X-axis
img2 = stackRotate90(img, 1, 1);

assertEqual(testCase, dim([3 2 1]), size(img2));

function testRotateGrayscaleZ(testCase)

img = createTestImageGray;
dim = size(img);

% rotate around Z-axis
img2 = stackRotate90(img, 3, 1);
assertEqual(testCase, dim([2 1 3]), size(img2));

function testColorImage(testCase)

% Create rgb image with non equal size in each dimension
lx = 1:50;
ly = 1:52;
lz = 1:54;
r = uint8(discreteBall(lx, ly, lz, [20 30 30], 15)*255);
g = uint8(discreteBall(lx, ly, lz, [30 20 30], 15)*255);
b = uint8(discreteBall(lx, ly, lz, [30 30 20], 15)*255);
img = imMergeChannels(r, g, b);

% basic checks
assertEqual(testCase, 'uint8', class(img));
assertEqual(testCase, [52 50 3 54], size(img));

% dimension of base image, without color
dim = size(r);

% rotate around Y-axis
img2 = stackRotate90(img, 2, 1);
assertEqual(testCase, [dim(1) dim(3) 3 dim(2)], size(img2));
assertEqual(testCase, img(1,1,:,1), img2(1, end,:,1));
assertEqual(testCase, img(1,1,:,end), img2(1, 1,:,1));

% rotate around X-axis
img2 = stackRotate90(img, 1, 1);
assertEqual(testCase, [dim(3) dim(2) 3 dim(1)], size(img2));

% rotate around Z-axis
img2 = stackRotate90(img, 3, 1);
assertEqual(testCase, [dim(2) dim(1) 3 dim(3)], size(img2));

function img = createBasicTestImage
img = cat(3, [1 2;3 4], [5 6;7 8]);

function img = createTestImageGray
% create a 3D gray-scale test image
%  ______
% |1    2|\
% | 5    |6|
% |3____4| |
%  \7____\8|
%
lx = 1:10;
ly = 30:5:60;
lz = 50:10:200;
[x, y, z] = meshgrid(lx, ly, lz);
img = uint8(x+y+z);


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;


classdef testNotCommutingHomogPlaneStress < ...
            testCommutingHomogPlaneStress

    methods (Access = protected, Static)

        function nu = createPoissonValue()
            nu = 1/3;
        end
    end

    methods (Access = public)

        function hasPassed = hasPassed(obj)
            c1 = obj.vhpTensor.getValue();
            c2 = obj.vphTensor.getValue();
            error = norm(c2-c1)/norm(c1);
            hasPassed = error > 1e-6;
        end

    end

end


function range = p00_range ( prob )

%*****************************************************************************80
%
%% P00_RANGE returns an interval bounding the root for any problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROB, the number of the problem.
%
%    Output, real RANGE(2), the minimum and maximum values of
%    an interval containing the root.
%
  if ( prob == 1 )
    range = p01_range ( );
  elseif ( prob == 2 )
    range = p02_range ( );
  elseif ( prob == 3 )
    range = p03_range ( );
  elseif ( prob == 4 )
    range = p04_range ( );
  elseif ( prob == 5 )
    range = p05_range ( );
  elseif ( prob == 6 )
    range = p06_range ( );
  elseif ( prob == 7 )
    range = p07_range ( );
  elseif ( prob == 8 )
    range = p08_range ( );
  elseif ( prob == 9 )
    range = p09_range ( );
  elseif ( prob == 10 )
    range = p10_range ( );
  elseif ( prob == 11 )
    range = p11_range ( );
  elseif ( prob == 12 )
    range = p12_range ( );
  elseif ( prob == 13 )
    range = p13_range ( );
  elseif ( prob == 14 )
    range = p14_range ( );
  elseif ( prob == 15 )
    range = p15_range ( );
  elseif ( prob == 16 )
    range = p16_range ( );
  elseif ( prob == 17 )
    range = p17_range ( );
  elseif ( prob == 18 )
    range = p18_range ( );
  elseif ( prob == 19 )
    range = p19_range ( );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_RANGE - Fatal error%\n' );
    fprintf ( 1, '  Illegal problem number = %d\n', prob );
    error ( 'P00_RANGE - Fatal error%' );
  end

  return
end


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