function [imageCha, ssim_map, metrics] = algo_FCSA_proxA_3D( obj,input )
% 3D variant of FCSA
% based on Huang et al. paper on FCSA
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1;
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.iNINNER - 5;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
nSlices = input.nSlices;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
flagRealAndImag = obj.flagRealAndImag;
waveletStages = obj.trafo.waveletStages;
maxWavecells = waveletStages*7+1;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
z_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2,nSlices);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = iFFT3D(b{1,j});
    y{1,j} = real(FTb{1,j}); % needed for y_old
    y{1,j+nCha} = imag(FTb{1,j});
end;
z = FTb; % starting point

x_wave = cell(1,2*nCha);
x_helper = x_wave;
x_wave_helper = x_wave;
x_tv = x_wave;
x_nltv = x_wave;
for j=1:2*nCha
    x_nltv{1,j} = 0;
end;
x_g = x_wave;
x_g_helper = x_wave;
x_g_proxA = x_wave;
z_comp = x_wave;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec_2D = cell(1,nCha);
    x_wavedec_3D = cell(1,nCha);
    threshold_2D = zeros(nCha,1); % one threshold value is enough (only for testing purposes)
    threshold_3D = zeros(nCha,1); % one threshold value is enough (only for testing purposes)
    if flagRealAndImag
        for j=1:nCha
            for l = 1:nSlices
                x_wavedec_2D{1,j} = wavedec2(abs(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wave_fine_scale_2D = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D:end),1);
            end;
            x_wavedec_3D{1,j} = wavedec3(abs(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_3D = zeros(0,0);
            for k = (7*(x_wavedec_3D{1,j}.level-1)+2):maxWavecells
                x_wave_fine_scale_3D = [x_wave_fine_scale_3D  x_wavedec_3D{1,j}.dec{k}];
            end;
            [size1, size2, size3] = size(x_wave_fine_scale_3D);
            threshold_3D(j) = mad(reshape(x_wave_fine_scale_3D,size1*size2*size3,1));
        end;
    else
        for j=1:nCha
            for l = 1:nSlices
                x_wavedec_2D{1,j} = wavedec2(real(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wavedec_2D{1,j+nCha} = wavedec2(imag(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wave_fine_scale_2D_real = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D_real:end),1);
                x_wave_fine_scale_2D_imag = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j+nCha) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D_imag:end),1);
            end;
            x_wavedec_3D{1,j} = wavedec3(real(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wavedec_3D{1,j} = wavedec3(imag(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_3D_real = zeros(0,0);
            x_wave_fine_scale_3D_imag = zeros(0,0);
            for k = (7*(x_wavedec_3D{1,j}.level-1)+2):maxWavecells
                x_wave_fine_scale_3D_real = [x_wave_fine_scale_3D_real  x_wavedec_3D{1,j}.dec{k+2}];
                x_wave_fine_scale_3D_imag = [x_wave_fine_scale_3D_imag  x_wavedec_3D{1,j}.dec{k+2}];
            end;
            [size1, size2, size3] = size(x_wave_fine_scale_3D_real);
            threshold_3D(j) = mad(reshape(x_wave_fine_scale_3D_real,size1*size2*size3,1));
            threshold_3D(j+nCha) = mad(reshape(x_wave_fine_scale_3D_imag,size1*size2*size3,1));
        end;
    end;
    clear x_wavedec
else
    threshold_2D(1:nCha) = 1;
    threshold_3D(1:nCha) = 1;
end;

if flagRealAndImag
    threshold_group = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold_3D(j)/2 * 2/L;
        threshold_TV(j) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_group = threshold_group + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group = threshold_group /3;
else
    threshold_group_real = 0;
    threshold_group_imag = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold_3D(j) * 2/L;
        threshold_wave(j+nCha) = lambdaWave * threshold_3D(j+nCha) * 2/L;
        threshold_TV(j) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold_2D(j+nCha) * 2/L;
        threshold_group_real = threshold_group_real + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_group_imag = threshold_group_imag + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group_real = threshold_group_real /3;
    threshold_group_imag = threshold_group_imag /3;
end;

%% initialize metrics:
itr = 0;
metrics.xtime(itr+1)= 0;
% [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma, nSlices );
ssim_map = [];

%% recon
dispProgress('Proximal Average', 0, maxitr);
for itr = 1:maxitr  % total iter counter

    t_new = (1+sqrt(1+4*t_old^2))/2;
    t_old = t_new;

    y_old = y;   % y_old = y for complex case

    %% landweber step
    for j = 1:nCha
        x_helper{1,j} = iFFT3D(FFT3D_mask(z{1,j},mask)) -FTb{1,j} ;
        z{1,j} = z{1,j} - x_helper{1,j}/L;
        z_comp{1,j} = real(z{1,j});
        z_comp{1,j+nCha} = imag(z{1,j});
    end;

    %% l1-Wavelet
    if flagWave
        for j = 1:2*nCha
            x_wave_helper{1,j} = wavedec3(z_comp{1,j},waveletStages,waveletFilterName_l1);
        end;
        if flagRealAndImag
            for j = 1:nCha
                for k = 1:maxWavecells
                    dec_cell{j}{k} = x_wave_helper{1,j}.dec{k,1};
                    dec_cell{j+nCha}{k} = x_wave_helper{1,j+nCha}.dec{k,1};
                    [c1, c2, c3] = size(dec_cell{j}{k});
                    [dec_cell{j}{k}, dec_cell{j+nCha}{k}] = groupthresh_2vec(reshape(dec_cell{j}{k},c1*c2*c3,1,1),reshape(dec_cell{j+nCha}{k},c1*c2*c3,1,1),threshold_wave(j));
                    x_wave_helper{1,j}.dec{k,1} = reshape(dec_cell{j}{k},c1,c2,c3);
                    x_wave_helper{1,j+nCha}.dec{k,1} = reshape(dec_cell{j+nCha}{k},c1,c2,c3);
                    dec_cell{j}{k} = [];
                    dec_cell{j+nCha}{k} = [];
                end;
            end;
        else
            for j = 1:2*nCha
                x_wave_helper{1,j} = softthresh_real(x_wave_helper{1,j},threshold_wave(j));
                for k = 1:maxWavecells
                    dec_cell{j}{k} = x_wave_helper{1,j}.dec{k,1};
                    [c1, c2, c3] = size(dec_cell{j}{k});
                    dec_cell{j}{k} = softthresh_real(reshape(dec_cell{j}{k},c1*c2*c3,1,1),threshold_wave(j));
                    x_wave_helper{1,j}.dec{k,1} = reshape(dec_cell{j}{k},c1,c2,c3);
                    dec_cell{j}{k} = [];
                end;
            end;
        end;
        for j = 1:2*nCha
            x_wave{1,j} = waverec3(x_wave_helper{1,j});
        end;
    end;

    %% TV
    if flagTV
        if ~flagTV_iso

            for j = 1:2*nCha
                for i = 1:nSlices
                    x_tv{1,j}(:,:,i) = MTV_2D(z_comp{1,j}(:,:,i),threshold_TV(j),n1,n2);
                end;
            end;
        else

            for j = 1:2*nCha
                for i = 1:nSlices
                    if (itr==1)
                        [x_tv{1,j}(:,:,i), P]=denoise_TV_One((z_comp{1,j}(:,:,i)), threshold_TV(j),-inf,inf,[],parsin);
                    else
                        [x_tv{1,j}(:,:,i), P]=denoise_TV_One((z_comp{1,j}(:,:,i)), threshold_TV(j),-inf,inf,P,parsin);
                    end;
                end;
            end;
        end;
    end;

    %% NLTV
    if flagNLTV
        if itr >= itrNLTV
            if flagSBNLTV
                for j = 1:2*nCha
                    for i = 1:nSlices
                        x_nltv{1,j}(:,:,i) = SB_NLTVfunc_slim_rescale(z_comp{1,j}(:,:,i),n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                    end;
                end;
            else
                for j = 1:2*nCha
                    for i = 1:nSlices
                        % if mod(itr,5) == 0 || itr == 1
                        x_nltv{1,j}(:,:,i) = NLMF(z_comp{1,j}(:,:,i),NLTV_struct);
                        x_nltv{1,j}(:,:,i) = (L.*z_comp{1,j}(:,:,i) + 2*threshold_NLTV(j)*x_nltv{1,j}(:,:,i))./(L+2*threshold_NLTV(j));
                        % end;
                    end;
                end;
            end;
        end;
    end;

    %% l12-Wavelet
    if flagGroup
        for j = 1:2*nCha
            for i = 1:nSlices
                if flag_extendImage
                    z_proxA{1,j}(:,:,i) = extend_image(z_comp{1,j}(:,:,i), waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                    x_g_helper{i}{1,j} = wavedec2(z_proxA{1,j}(:,:,i),waveletStages,waveletFilterName_l12);
                else
                    x_g_helper{i}{1,j} = wavedec2(z_comp{1,j}(:,:,i),waveletStages,waveletFilterName_l12);
                end;

                if flag_wavetree
                    x_g_helper{i}{2,j} = (G_prox*x_g_helper{i}{1,j}')';
                else
                    x_g_helper{i}{2,j} = zeros(1,size(x_g_helper{i}{1,j}(:,:,i),2));
                end;
            end;
        end;
        for i = 1:nSlices
            if flagRealAndImag
                x_g_helper{i}(1,:) = softthresh_proxA_cha(x_g_helper{i}(:,:),threshold_group,2*nCha,waveS_l12proxA,groupnorm_index);
            else
                x_g_helper{i}(1,1:nCha) = softthresh_proxA_cha(x_g_helper{i}(:,1:nCha),threshold_group_real,nCha,waveS_l12proxA,groupnorm_index);
                x_g_helper{i}(1,nCha+1:2*nCha) = softthresh_proxA_cha(x_g_helper{i}(:,1:nCha),threshold_group_imag,nCha,waveS_l12proxA,groupnorm_index);
            end;
        end;
        for j = 1:2*nCha
            for i = 1:nSlices
                x_g_proxA{1,j}(:,:,i) = waverec2(x_g_helper{i}{1,j},waveS_l12proxA,waveletFilterName_l12);
                x_g{1,j}(:,:,i) =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x,i);
            end;
        end;
    end;

    %% add prox(.)
    for j = 1:2*nCha
        y{1,j} = zeros(n1,n2,nSlices);
        if flagWave y{1,j} = y{1,j} + x_wave{1,j}.*regularizerWeights(1); end;
        if flagTV y{1,j} = y{1,j} + x_tv{1,j}.*regularizerWeights(2); end;
        if flagGroup y{1,j} = y{1,j} + x_g{1,j}.*regularizerWeights(3); end;
        if flagNLTV y{1,j} = y{1,j} + x_nltv{1,j}.*regularizerWeights(4); end;

        if itr < itrNLTV
            y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
        else
            y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagNLTV.*regularizerWeights(4) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
        end;

        if flag_fast
            y{1,j}=y{1,j}+((t_old-1)/t_new).*(y{1,j}-y_old{1,j});
        end;
    end;

    %% metrics of current itr:
%     disp(itr);
    dispProgress('Proximal Average', itr/maxitr);

    metrics.xtime(itr+1)= toc(timer_proxA);
    for j = 1:nCha
        z{1,j} = y{1,j} + 1i*y{1,j+nCha};
    end;
%     [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma, nSlices );

end;
dispProgress('Proximal Average', 'Close');

imageCha = z;
for j = 1:nCha
    imageCha{1,j} = turn_image( imageCha{1,j} );
end;
% for j = 1:nCha+1
%     ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%     ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
% end;

end


function X=pathSolutionLeast(A, y, z, opts)
%
%% Fuction pathSolution:
%      Solving the pathwise solutions
%
%% Input & Output parameters
%  See the description of the related functions
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified 2 August 2009.
%
% Related functions:
%  sll_opts, initFactor,
%  eppVector, eppMatrix, eplb,
%
%  LeastR, LeastC,
%  nnLeastR, nnLeastC
%  glLeastR,  mtLeastR, mcLeastR
%%

switch(opts.fName)
    case 'LeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=LeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastR
            [x, funVal]=LeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'LeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=LeastC(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastC
            [x, funVal]=LeastC(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'glLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=glLeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function glLeastR
            [x, funVal]=glLeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'mtLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        k=length(opts.ind)-1;        % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mtLeastR(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mtLeastR
            [x, funVal]=mtLeastR(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'mcLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        k=size(y,2);                 % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mcLeastR(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=1;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mcLeastR
            [x, funVal]=mcLeastR(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'nnLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=nnLeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastR
            [x, funVal]=nnLeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'nnLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=nnLeastC(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastC
            [x, funVal]=nnLeastC(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'mtLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        k=length(opts.ind)-1;        % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mtLeastC(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mtLeastC
            [x, funVal]=mtLeastC(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'mcLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        k=size(y,2);                 % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mcLeastC(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=1;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mcLeastC
            [x, funVal]=mcLeastC(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    otherwise
        fprintf('\n The function value specified in opts.fName is not supported!');
end


%% This file is the main file of using the SINDy-PI method to
% infer the Yeast Glycolysis Model. We will figure out what is the minimum
% data length needed for SINDy-PI to accurately discover the six-th state
% of the Yeast Glycolysis Model. This file will be used to swipe through
% different data length.
%
% Date: 2019/06/13
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
set(0,'defaulttextInterpreter','latex')
addpath('Functions')
addpath('Datas')
%% Define some parameters
% Define whehter you have control, if you have it, please define it
n_control=0;u=0;

% Run the ODE files and gather the simulation data.
% (We use the same data for both the iSINDy method and SINDy-PI method for better comparision)
% (Baseline data for the data length comparison of state 1,2,3,4,5,6,7)
% The data is noise clean. It is generated using 900 different initial
% conditions with 51 points of each initial condition.
load('TrainingData.mat')

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Define how many states we have in our example
n_state=7;

% Print the actual ODE we try to discover
digits(4)
Print_ODEs(@(t,y)YeastGlycolysis_ODE(t,y),n_state,n_control,disp_actual_ode,actual);

% Create symbolic states
dz=sym('dz',[n_state,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=0;
Highest_Trig_Order_Guess=0;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Trig_Order=0;
Highest_U_Order=0;
Highest_dPoly_Order=1;

% Set the parameter normLib=1 to normalize the librry
normLib=1;

% Set how amny iterations you want for the sparse regression
N_iter=10;

% Set whether you want to display the ODE or not
disp=0;

% Set the library size
Highest_Poly_Order_Lib=[6;6;3;3;3;6;3];

% Determine how many percent of data you want.
percent_start=1;d_percent=0.005;percent_end=12;

% Determine which states you want to discover
Which_State=6;

% Determine the left hand side guess number
if Which_State==1 || Which_State==2 || Which_State==6
    LHS_Num=2;
else
    LHS_Num=1;
end

% Get the poly-order
Highest_Poly_Order=Highest_Poly_Order_Lib(Which_State);

% Determine which LHS you want to use, the first one or the second one
LHS_Pin=1;
%%
tic
% Create the library data using all the data points
[SINDy_Data_Full,SINDy_Struct]=SINDyLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);
fprintf('\n\t Original library creation finished, using %i seconds...\n',toc)

% Create left hand side guess
[LHS_Data_Full,LHS_Sym]=GuessLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order_Guess,Highest_Trig_Order_Guess,Highest_U_Order_Guess);

tic
% Create the right hand side, exclude the guess from SINDy library
[RHS_Data_Full,RHS_Struct]=ExcludeGuess(SINDy_Data_Full,SINDy_Struct,LHS_Sym{LHS_Pin});
LHS_Data_Full_Dum=LHS_Data_Full(:,LHS_Pin);
fprintf('\t Library without LHS guess creation finished, using %i seconds...\n',toc)

% Determine sparsity parameter
lambda=0.1;

% Create the new directory to save the result
FolderName=strcat('Result_DL_SINDy_Data_Length_Compare_State_',num2str(Which_State),'_LHS_Guess_',num2str(LHS_Pin),'_SwipeLength_',num2str(lambda));
[fld_status, fld_msg, fld_msgID]=mkdir(FolderName);

% Set up dummy variables for parfor
LHS_Sym_Dum=LHS_Sym{LHS_Pin};
dz_dum=dz(Which_State);
%% determine the percentage you want to use
if LHS_Pin==1
    Percent_Iter=[0.30 0.31 0.32 0.33 0.34 0.345 0.35 0.355 0.36 0.365 0.37 0.38 0.39 0.40];
else
    Percent_Iter=[0.02 0.03 0.04 0.05 0.055 0.0575 0.06 0.0625 0.065 0.0675 0.07 0.0725 0.075 0.08 0.09 0.1 0.2];
end

for per=1:size(Percent_Iter,2)
    percent=Percent_Iter(per);

    % Start!
    for Total_Run=1:10
        fprintf('\n \n Get the result for the %i time...\n',Total_Run)

        % Print the state we are testing
        fprintf('\n \t Calculating the %i expression...\n',Which_State)

        % Print the left hand side that we are testing
        fprintf('\t Testing the left hand side as %s:\n',char(LHS_Sym_Dum))

        % Define the new data length
        new_length=round((percent)*length(xt));

        % Shuffel the original data
        Sequence=randperm(size(SINDy_Data_Full,1));
        Sequence_Trimed=Sequence(1:round((percent)*length(xt)));

        RHS_Data=RHS_Data_Full(Sequence_Trimed,:);
        LHS_Data=LHS_Data_Full_Dum(Sequence_Trimed,1);

        fprintf('\n \t Data preparation finished, using %i seconds...\n',toc)

        % We fix the lambda and change the data usage.
        tic
        fprintf('\n \t Testing the percentage as %i ...\n',(percent*100))

        % Perform the sparse regression problem
        Xi=sparsifyDynamics_simplified(RHS_Data,LHS_Data,lambda,N_iter,normLib);
        fprintf('\t Uses %i seconds...\n',toc)

        % Save the calculation result of current iteration
        fprintf('\n\t Saving the result...\n')
        tic
        cc=clock;
        ResultName=strcat(FolderName,'/DL_SINDY_Data_Length_',num2str(Total_Run),'__','LHS',num2str(LHS_Pin),'_',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'_P3','.mat');
        save(ResultName,'Xi','lambda','percent')
        fprintf('\n\t Saving finished! Using %i seconds...\n',toc)
    end
end


function r = roots( f, g, varargin )
%ROOTS   Zeros of a SEPARABLEAPPROX.
%   R = ROOTS(F) returns the zero contours of F as a quasimatrix of chebfuns.
%   Each column of R is one zero contour.  This command only finds contours when
%   there is a change of sign, and it can also group intersecting contours in a
%   non-optimal way.
%
%   For a faster plot to graphical accuracy use CONTOUR(F, [0 0]).
%
%   R = ROOTS(F, G) returns the isolated mutual roots of F and G.
%
%   R = ROOTS(F, G, METHOD) allows the underlying rootfinding algorithm to
%   be specified.  If METHOD = 'ms' or 'marchingsquares', the Marching
%   Squares algorithm is employed, which is fast but not very robust.
%   If METHOD = 'resultant', a hidden variable resultant method
%   based on Bezout resultants is employed, slower but more robust.
%   See the CHEBFUN2V/ROOTS documentation to see which algorithm is used
%   when no METHOD is passed.
%
% Example:
%   cheb.xy;
%   f = x.^2 + y.^2 - 1/4;
%   roots(x,f)                               % [0 -.5; 0 .5]
%   c = roots(f);
%   arclength = sum(abs(diff(c)))            % pi
%   area = abs(sum(real(c).*diff(imag(c))))  % pi/4
%
% See also CHEBFUN2V/ROOTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Check for empty chebfun input:
if ( isempty( f ) )
    r = [];
    return
end

dom = f.domain;
scl = norm(dom,inf);

if ( nargin == 1 )  % Seek zero curves of scalar function

    if ( length( f ) == 1 )  % Special case: f has rank 1:
        cols = f.cols;
        rows = f.rows;
        yrts = 1i*(roots( cols )+realmin);
        xrts = roots( rows ) + realmin*1i; % Add complex to ensure it's not real
        r = chebfun;
        % Go though col(yrts) = 0, make into a horizontal line:
        dom = rows.domain;
        len = (dom(2)-dom(1))/2;
        for j = 1 : numel(yrts)
            f = chebfun(@(x) (len*(x+1)+dom(1)) + yrts(j));
            r = [r f];
        end
        % Go though row(xrts) = 0, make into a vertical line:
        dom = cols.domain;
        len = (dom(2)-dom(1))/2;
        for j = 1 : numel(xrts)
            f = chebfun(@(x) 1i*(len*(x+1)+dom(1)) + xrts(j));
            r = [r f];
        end

    elseif ( isreal( f ) )  % Main case: zero curves for real function
        % We seek accurate chebfun representations of each component
        % of the zero curve.  First we call Matlab's contourc function
        % to get initial data points, which typically will lie on the zero
        % curve to 5-6 digit accuracy and will be smoothly spaced along
        % it to 2-3 digit accuracy.  The choice n = 502 is used to get a
        % grid that does not involve the boundary of the domain.
        n = 502;
        x = linspace( dom(1), dom(2), n );
        x(1) = []; x(end) = [];
        y = linspace( dom(3), dom(4), n );
        y(1) = []; y(end) = [];
        [xx, yy] = meshgrid( x, y );
        vals = feval( f, xx, yy );
        C = contourc( x, y, vals, 0*[1 1] );
        [fx, fy] = grad(f);
        gradf = @(data) feval( fx, real(data), imag(data)) ...
                   + 1i*feval( fy, real(data), imag(data));
        fval = @(data) feval( f, real(data), imag(data));

        % The construction of chebfuns proceeds by improving the accuracy
        % of the curves component by component, using complex arithmetic
        % for convenience.
        j = 1; r = chebfun;
        while ( j < length(C) )
            k = j + C(2, j);
            D = C(:, j+1:k);
            data = ( D(1, :) + 1i*(D(2, :)+realmin) ).';
            ii = find(abs(diff(data)) < 1e-8*scl);
            data(ii) = [];                           % eliminate repetitions
            npts = length(data);
            err = 999; errnew = 1e-2;
            stepno = 0;
            curvenew = chebfun(data);
            while (errnew < err) && (stepno < 6)
                stepno = stepno+1;
                err = errnew;
                curve = curvenew;
                s = [0; cumsum(abs(diff(data)))];    % empirical arclength
                s = 2*s/s(end) - 1;                  % normalize to [-1,1]
                data = interp1(s, data, chebpts ...  % interpolate
                    (length(data)), 'spline');
                curvenew = chebfun(data);            % make chebfun
                data = curvenew(chebpts(npts));      % sample finely
                data = snap(data);                   % snap to boundary
                ff = fval(data);                     % function values
                g = gradf(data);                     % gradient values
                errnew = norm(ff,inf)/vscale(f);     % max rel error
                data = data - ff.*( g./abs(g).^2 );  % Newton step
                curvenew = chebfun(data);            % make chebfun
                curvenew = simplify(curvenew);       % simplify it
            end
            j = k + 1;
            r = [ r , curve ];
        end

    else  % Function is complex-valued: reduce to real case
        r = roots( [ real(f) ; imag(f) ] );
        if ( ~isempty( r ) )
            r = r(:, 1) + 1i*r(:, 2);
        end
    end

elseif ( isa(g, 'separableApprox') )  % seek zero points of vector function
    % TODO: This should not call chebfun2v directly.
    r = roots( chebfun2v( f, g ), varargin{:} );

end

function d = snap(d)   % adjust endpoints to snap to boundary of domain
    n = length(d);

    if abs(real(d(1))-dom(2)) < .02*scl                 % snap to right bndry
        dx0 = dom(2)-real(d(2)); dx = real(d(1)-d(2));
        d(1) = d(2) + (dx0/dx)*(d(1)-d(2));
    end
    if abs(real(d(n))-dom(2)) < .02*scl
        dx0 = dom(2)-real(d(n-1)); dx = real(d(n)-d(n-1));
        d(n) = d(n-1) + (dx0/dx)*(d(n)-d(n-1));
    end

    if abs(real(d(1))-dom(1)) < .02*scl                 % snap to left bndry
        dx0 = dom(1)-real(d(2)); dx = real(d(1)-d(2));
        d(1) = d(2) + (dx0/dx)*(d(1)-d(2));
    end
    if abs(real(d(n))-dom(1)) < .02*scl
        dx0 = dom(1)-real(d(n-1)); dx = real(d(n)-d(n-1));
        d(n) = d(n-1) + (dx0/dx)*(d(n)-d(n-1));
    end

    if abs(imag(d(1))-dom(4)) < .02*scl                 % snap to top bndry
        dy0 = dom(4)-imag(d(2)); dy = imag(d(1)-d(2));
        d(1) = d(2) + (dy0/dy)*(d(1)-d(2));
    end
    if abs(imag(d(n))-dom(4)) < .02*scl
        dy0 = dom(4)-imag(d(n-1)); dy = imag(d(n)-d(n-1));
        d(n) = d(n-1) + (dy0/dy)*(d(n)-d(n-1));
    end

    if abs(imag(d(1))-dom(3)) < .02*scl                 % snap to bottom bndry
        dy0 = dom(3)-imag(d(2)); dy = imag(d(1)-d(2));
        d(1) = d(2) + (dy0/dy)*(d(1)-d(2));
    end
    if abs(imag(d(n))-dom(3)) < .02*scl
        dy0 = dom(3)-imag(d(n-1)); dy = imag(d(n)-d(n-1));
        d(n) = d(n-1) + (dy0/dy)*(d(n)-d(n-1));
    end

end

end


function varargout=resgram(f,varargin)
%RESGRAM  Reassigned spectrogram plot
%   Usage: resgram(f,op1,op2, ... );
%          resgram(f,fs,op1,op2, ... );
%
%   `resgram(f)` plots a reassigned spectrogram of *f*.
%
%   `resgram(f,fs)` does the same for a signal with sampling rate *fs*
%   (sampled with *fs* samples per second).
%
%   Because reassigned spectrograms can have an extreme dynamical range,
%   consider always using the `'dynrange'` or `'clim'` options (see below)
%   in conjunction with the `'db'` option (on by default). An example:::
%
%     resgram(greasy,16000,'dynrange',70);
%
%   This will produce a reassigned spectrogram of the |greasy| signal
%   without drowning the interesting features in noise.
%
%   `C=resgram(f, ... )` returns the image to be displayed as a matrix. Use this
%   in conjunction with `imwrite` etc. These coefficients are **only** intended to
%   be used by post-processing image tools. Reassignment should be done
%   using the |gabreassign| function instead.
%
%   `resgram` accepts the following additional arguments:
%
%
%     'dynrange',r  Limit the dynamical range to *r* by using a colormap in
%                   the interval $[chigh-r,chigh]$, where *chigh* is the highest
%                   value in the plot. The default value of `[]` means to not
%                   limit the dynamical range.
%
%     'db'         Apply `20*log10` to the coefficients. This makes it possible to
%                  see very weak phenomena, but it might show too much noise. A
%                  logarithmic scale is more adapted to perception of sound.
%                  This is the default.
%
%     'sharp',alpha
%                  Set the sharpness of the plot. If $alpha=0$ the regular
%                  spectrogram is obtained. $alpha=1$ means full
%                  reassignment. Anything in between will produce a partially
%                  sharpened picture. Default is $alpha=1$.
%
%     'lin'        Show the energy of the coefficients on a linear scale.
%
%     'tfr',v      Set the ratio of frequency resolution to time resolution.
%                  A value $v=1$ is the default. Setting $v>1$ will give better
%                  frequency resolution at the expense of a worse time
%                  resolution. A value of $0<v<1$ will do the opposite.
%
%     'wlen',s     Window length. Specifies the length of the window
%                  measured in samples. See help of |pgauss| on the exact
%                  details of the window length.
%
%     'posfreq'    Display only the positive frequencies. This is the
%                  default for real-valued signals.
%
%     'nf'         Display negative frequencies, with the zero-frequency
%                  centered in the middle. For real signals, this will just
%                  mirror the upper half plane. This is standard for complex
%                  signals.
%
%     'tc'         Time centering. Move the beginning of the signal to the
%                  middle of the plot. This is useful for visualizing the
%                  window functions of the toolbox.
%
%     'image'      Use `imagesc` to display the spectrogram. This is the
%                  default.
%
%     'clim',clim  Use a colormap ranging from $clim(1)$ to $clim(2)$. These
%                  values are passed to `imagesc`. See the help on `imagesc`.
%
%     'thr',r      Keep only the largest fraction *r* of the coefficients, and
%                  set the rest to zero.
%
%     'fmax',y     Display *y* as the highest frequency. Default value of `[]`
%                  means to use the Nyquist frequency.
%
%     'xres',xres  Approximate number of pixels along x-axis / time.
%                  The default value is 800
%
%     'yres',yres  Approximate number of pixels along y-axis / frequency
%                  The default value is 600
%
%     'contour'    Do a contour plot to display the spectrogram.
%
%     'surf'       Do a surf plot to display the spectrogram.
%
%     'mesh'       Do a mesh plot to display the spectrogram.
%
%     'colorbar'   Display the colorbar. This is the default.
%
%     'nocolorbar'  Do not display the colorbar.
%
%   In addition to these parameters, `sgram` accepts any of the flags from
%   |setnorm|. The window used to calculate the spectrogram will be
%   normalized as specified.
%
%   See also: sgram

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: NA
%   REFERENCE: NA

% BUG: Setting the sharpness different from 1 produces a black line in the
% middle of the plot.

if nargin<1
  error('Too few input arguments.');
end;

if sum(size(f)>1)>1
  error('Input must be a vector.');
end;

definput.import={'ltfattranslate','setnorm','tfplot'};

% Define initial value for flags and key/value pairs.
definput.flags.wlen={'nowlen','wlen'};
definput.flags.thr={'nothr','thr'};
definput.flags.tc={'notc','tc'};
definput.flags.plottype={'image','contour','mesh','pcolor'};

% Override the setting from tfplot, because SGRAM does not support the
% 'dbsq' setting (it does not make sense).
definput.flags.log={'db','lin'};

if isreal(f)
  definput.flags.posfreq={'posfreq','nf'};
else
  definput.flags.posfreq={'nf','posfreq'};
end;

definput.keyvals.sharp=1;
definput.keyvals.tfr=1;
definput.keyvals.wlen=0;
definput.keyvals.thr=0;
definput.keyvals.fmax=[];
definput.keyvals.xres=800;
definput.keyvals.yres=600;

[flags,kv,fs]=ltfatarghelper({'fs','dynrange'},definput,varargin);

if (kv.sharp<0 || kv.sharp >1)
  error(['RESGRAM: Sharpness parameter must be between (including) ' ...
	 '0 and 1']);
end;

% Downsample
if ~isempty(kv.fmax)
  if ~isempty(kv.fs)
    resamp=kv.fmax*2/fs;
  else
    resamp=kv.fmax*2/length(f);
  end;

  f=fftresample(f,round(length(f)*resamp));
  kv.fs=2*kv.fmax;
end;

Ls=length(f);

if flags.do_posfreq
   kv.yres=2*kv.yres;
end;

try
  [a,M,L,N,Ndisp]=gabimagepars(Ls,kv.xres,kv.yres);
catch
  err=lasterror;
  if strcmp(err.identifier,'LTFAT:noframe')
    error(sprintf(['The signal is too long. RESGRAM cannot visualize all the details.\n' ...
                   'Try a shorter signal or increase the image resolution by calling:\n\n' ...
                   '  sgram(...,''xres'',xres,''yres'',yres);\n\n' ...
                   'for larger values of xres and yres.\n'...
                   'The current values are:\n  xres=%i\n  yres=%i'],kv.xres,kv.yres));
  else
    rethrow(err);
  end;
end;

% Set an explicit window length, if this was specified.
if flags.do_wlen
  kv.tfr=kv.wlen^2/L;
end;

g={'gauss',kv.tfr,flags.norm};

[tgrad,fgrad,c]=gabphasegrad('dgt',f,g,a,M);
coef=gabreassign(abs(c).^2,kv.sharp*tgrad,kv.sharp*fgrad,a);

% Cut away zero-extension.
coef=coef(:,1:Ndisp);

if flags.do_thr
  % keep only the largest coefficients.
  coef=largestr(coef,kv.thr);
end

if flags.do_posfreq
  coef=coef(1:floor(M/2)+1,:);
  plotdgtreal(coef,a,M,'argimport',flags,kv);
else
  plotdgt(coef,a,'argimport',flags,kv);
end;

if nargout>0
  varargout={coef};
end;


function [X,Y,Z,CAPS] = extrude(varargin)
% EXTRUDE  Extrude a 2D curve along a 3D path to create
% tubes/cylinders/etc (and, optionally, fly though it!)
%
%   [X,Y,Z] = EXTRUDE(base,traj)
%   [X,Y,Z,CAPS] = EXTRUDE(base,traj,cap)
%   [X,Y,Z,CAPS] = EXTRUDE(base,traj,cap,alg)
%   [X,Y,Z,CAPS] = EXTRUDE(base,traj,cap,alg,fly) returns the surface and
%   handle to the surface plot object created by the 2D base curve and 3D
%   trajectory. SURF(X,Y,Z) can be used to display the object. If cap == 2,
%   data for the caps will be sent separately.
%
%   base = [x;y]   A 2-row Matrix defining the 2d base
%   traj = [x;y;z] A 3-row Matrix defining the 3d path
%   cap = Close off the ends of the tube:
%            0 -> No (default)
%            1 -> Yes
%            2 -> Yes but output them separately into the variable CAPS
%
%   alg = Creation algorithm:
%            1 Creates a natural looking surface with little twist (default)
%            2 More twisty, but preserves orientation vs. direction (which
%              is better for periodicity)
%   fly =  N  Fly through the path generated N times
%
%
%   DEMO: EXTRUDE with no arguments flies through a random periodic path
%         with a star/road/tube cross section using either algorithm
%
%   EXAMPLE 1: Generate a torus:
%
%       q = linspace(0,2*pi,33);
%       base = [cos(q); sin(q)];   % Base curve is a circle (radius = 1)
%       q = linspace(0,2*pi,101);
%       traj = 5*[cos(q); sin(q); 0*q]; %Trajectory is a circle (radius = 5)
%
%       [X,Y,Z] = extrude(base,traj);
%       figure; surf(X,Y,Z); axis equal;
%
%   EXAMPLE 2: Generate half a torus, with nice caps plotted separately:
%
%       q = linspace(0,2*pi,33);
%       base = [cos(q); sin(q)];   % Base curve is a circle (radius = 1)
%       q = linspace(0,pi,101);
%       traj = 5*[cos(q); sin(q); 0*q]; %Trajectory is a semicircle (radius = 5)
%
%       [X,Y,Z,CAPS] = extrude(base,traj,2);    %cap = 2 for separate caps
%       figure; surf(X,Y,Z); axis equal; hold on;
%       surf(CAPS(1).X,CAPS(1).Y,CAPS(1).Z,'linestyle','none');
%       surf(CAPS(2).X,CAPS(2).Y,CAPS(2).Z,'linestyle','none');
%
%
%   EXAMPLE 3: Generate a 3D square tube and fly through it once:
%
%       q = linspace(0,2*pi,5) + pi/4;
%       base = 0.15*[cos(q); sin(q)];   % Base curve is a square
%       t = linspace(0,1,2001);
%       traj = [sin(4*pi*t+1); sin(5*pi*t+2); sin(6*pi*t)];
%
%       [X,Y,Z] = extrude(base,traj,0,[],1);
%       figure; surf(X,Y,Z); axis equal;
%
%   2009-12-25 v3.1     Fixed an error with argument parsing
%   2009-12-18 v3.0     Added an option to close off the ends, and changed
%                       the order of the input arguments.
%   2009-08-20 v2.0     Added a better algorithm that allow for much less
%                       twistiness, a new (simpler) example, and better
%                       error checking.
%   2009-08-20 v1.1     Fixed an error with passing in only 2 arguments and
%                       clearer documentation
%   2009-08-19 v1.0     Initial Creation

%% Process input parameters
if nargin < 2  % Default base = star,   Default path = random
    disp('EXTRUDE DEMO: Fly through a random periodic path.')
    sw =  input('Pick a cross-section: (1)Star (2)Road (3)Tube ? ');
    switch sw
        case 1
            bs = 5; sharp = 0.5; bsize = .1;   %5 pointed star...
            basex = [cos(linspace(0,2*pi,bs+1));
                    sharp*cos(2*pi/2/bs + linspace(0,2*pi,bs+1))];
            basey = [sin(linspace(0,2*pi,bs+1));
                    sharp*sin(2*pi/2/bs + linspace(0,2*pi,bs+1))];
            base = bsize*[basex(1:2*bs+1); basey(1:2*bs+1)];
        case 2
            base = [-1 -1 1 1; 0 -.2 -.2 0]/10;
        case 3
            base = .1*[cos(linspace(0,2*pi,12)); sin(linspace(0,2*pi,12))];
        otherwise
            disp('Pick 1, 2, or 3! Try again.'); return;
    end

    alg = input('Pick an algorithm: (1) Normal (2) Twisty but orientation preserving ? ');
    if alg ~= 1 && alg ~= 2
        disp('Pick 1 or 2! Try again.'); return;
    end

    C = interpft(randn(10,3),1001)'; %Generate a smooth periodic path
    C = [C(:,end) C]; %close the loop
    C = 1*C./max(abs(C(:))); %normalize it
	fly = Inf;   %Fly through it forever
    cap = 0;    % No caps

elseif nargin >= 2;
    fly = []; alg = 1; cap = 0;
    C = varargin{2};
    base = varargin{1};
    if nargin >= 3 && ~isempty(varargin{3}); cap = varargin{3}; end
    if nargin >= 4 && ~isempty(varargin{4}); alg = varargin{4}; end
    if nargin == 5 && ~isempty(varargin{5}); fly = varargin{5}; end
end

% Check size to make size(C) = 3xN
if size(C,2) == 3 && size(C,1) ~= 3
    C = C';
end

% Check size to make size(base) = 2xN
if size(base,2) == 2 && size(base,1) ~= 2
    base = base';
end

%% Calculate derivatives and allocate matrices
npt = size(base,2);
base = [base; zeros(1,npt)];

if size(C,2) >= 3 %Use a 2nd order approximation for the derivatives of the trajectory
    dC = [C(:,1:3)*[-3; 4; -1]/2  [C(:,3:end) - C(:,1:end-2)]/2 C(:,end-2:end)*[1; -4; 3]/2];
else
    dC = C(:,[2 2]) - C(:,[1 1]);
end

dC0 = find(sum(abs(dC),1) == 0,1);    %Check for stagnation points
if ~isempty(dC0)
    warning('Removing stagnation points found in trajectory');
    dCgood = find(sum(abs(dC),1) ~= 0);
    C = C(:,dCgood); %
    dC = dC(:,dCgood); %
end

K = size(dC,2);
SUR = nan(3,npt,K);
camtar = zeros(3,K);
camup = zeros(3,K);
camdata = [[0;0;1],[0;1;0]];
%% Generate and plot the surface

dCvec_prev = [0;0;1];

switch alg
    case 1
        %% Natural Looking Tube (default)
        for k = 1:K
            % We want to rotate [0;0;1] 180degrees around an axis 'z' to become dC

            dCvec = dC(:,k)/norm(dC(:,k));
            z = cross(dCvec_prev,dCvec);

            if norm(z) ~= 0
                z = z/norm(z);
                q = real(acos(dot(dCvec_prev,dCvec)/norm(dCvec_prev)/norm(dCvec)));

                Z = repmat(z,1,npt);
                base = base*cos(q) + cross(Z,base)*sin(q)+Z*(1-cos(q))*diag(dot(Z,base));
                camdata = camdata*cos(q) + cross([z z],camdata)*sin(q)+[z z]*(1-cos(q))*diag(dot([z z],camdata));

                dCvec_prev = dCvec;
            end

            % Data for the camera used in the flying routine
            camtar(:,k) = camdata(:,1);
            camup(:,k) = camdata(:,2);

            SUR(:,:,k) = base + repmat(C(:,k),1,npt);

        end
    case 2
        %% Orientation preserving tube (more twisty but allows for periodicity)
        for k = 1:K
            % We want to rotate [0;0;1] 180degrees around an axis 'z' to become dC
            dCvec = dC(:,k)/norm(dC(:,k));
            if isequal(dCvec,[0;0;-1]) %Prevents a 0/0 = nan
                z = [0;1;0];
            else
                z = ([0; 0; 1] + dCvec)/2; z = z/norm(z);
            end
            z = repmat(z,1,npt);

            % Data for the camera used in the flying routine
            camtar(:,k) = z(:,1)*z(3)*2 - [0;0;1];
            camup(:,k) = z(:,1)*z(2)*2 - [0;1;0];

            SUR(:,:,k) = z*diag(dot(z,base))*2 - base + repmat(C(:,k),1,npt);

        end
    otherwise, error('Algorthm 1 or 2?');
end

CAPS = [];
if cap == 1; % Add caps
    SUR = cat(3,repmat(C(:,1),1,npt),SUR,repmat(C(:,K),1,npt));
end

X = squeeze(SUR(1,:,:));
Y = squeeze(SUR(2,:,:));
Z = squeeze(SUR(3,:,:));

if cap == 2 % Add separate caps
    SURCAP1 = cat(3,SUR(:,:,1),repmat(C(:,1),1,npt));
    SURCAP2 = cat(3,SUR(:,:,K),repmat(C(:,K),1,npt));
    CAPS(1).X = squeeze(SURCAP1(1,:,:));
    CAPS(1).Y = squeeze(SURCAP1(2,:,:));
    CAPS(1).Z = squeeze(SURCAP1(3,:,:));
    CAPS(2).X = squeeze(SURCAP2(1,:,:));
    CAPS(2).Y = squeeze(SURCAP2(2,:,:));
    CAPS(2).Z = squeeze(SURCAP2(3,:,:));
end

if isempty(fly) || fly <= 0
    return
end

if exist('sw','var') %If in DEMO mode, make two figures
    surf(X,Y,Z);
    axis equal;
end

curfig = figure;
h = surf(X,Y,Z);
axis equal;

%% Fix any NaN's (This should not happen...)
xisnan = find(isnan(X(1,:)), 1);
xnotnan = find(~isnan(X(1,:)));
if ~isempty(xisnan)
    warning('NaN''s found');
    X = X(:,xnotnan);
    Y = Y(:,xnotnan);
    Z = Z(:,xnotnan);
    camup = camup(:,xnotnan);
    camtar = camtar(:,xnotnan);
    C = C(:,xnotnan);
end

%% Fly through it
set(h,'facealpha',0.5,'edgecolor','flat'); colormap(hsv); %Make it look nice
set(gca,'visible','off','projection','perspective');
set(gcf,'color','k');
set(gcf,'closerequestfcn','assignin(''caller'',''stopme'',1); delete(gcbf)'); % Stop the loop when closing the figure
h = gcf;
K = size(C,2);
k = 0;
laps = 1;
stopme = 0;

while laps <= ceil(fly) && gcf == curfig && stopme == 0
    k = 1 + mod(k,K);  %Increment k but loop from K back to 1

    set(gca,'cameraviewangle',150);
    set(gcf,'name',['Lap ' num2str(laps) '/' num2str(fly) '  ' num2str(k) '/' num2str(K) '   Close the figure to stop.']);
    set(gca,'cameraposition',C(:,k),'cameratarget',C(:,k)+camtar(:,k),'cameraupvector',camup(:,k));
    drawnow;

    if k == K; laps=laps+1; end;

end
try close(h); catch; end; % If the figure remains after leaving the while loop


function [coeff,resp,modelCI,medianHR] = computeModelCoefficientsTime_HN(X,Y,censoring,seed)
% -------------------------------------------------------------------------
% function [coeff,resp,modelCI] = computeModelCoefficients(X,Y,imbalance,batchNum)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the final model logistic regression coefficients
% using bootstrap resampling, and it associated multivariable model
% response and bootstrap confidence intervals. See ref. [1] for more
% details. This function uses logistic regression utilities from DREES
% <http://www.cerr.info/drees>, as well as a rounding function written by
% Francois Beauducel available at:
% <http://www.mathworks.com/matlabcentral/fileexchange/26212-round-with-significant-digits>
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% [2] Vallieres, M. et al. (2015).
% -------------------------------------------------------------------------
% INPUTS:
% - X: Matrix of size [nInst X nFeat], specifying the numerical data of the
%      features of the input features, where 'nInst' refers to the number
%      of instances in X, and 'nFeat' to the number of features in X.
%      Each column is a different feature.
% - Y: Column vector of size [nInst X 1] specifying the outcome status
%      (1 or 0) for all instances.
% - imbalance: String specifying the type of imbalance-adjustement strategy
%              employed. Either 'IABR' for imbalance-adjusted bootstrap
%              resampling (see ref.[1]), or 'IALR' for imbalance-adjusted
%              logistic regression (see ref.[2]).
% - batchNum: (optional input). If present, integer that specifies the
%             batch number for parallelization purposes.
% -------------------------------------------------------------------------
% OUTPUTS:
% - coeff: Column vector of size [nCoeff+1 X 1] specifying the final
%          logistic regression coefficients. Last entry specify the offset
%          of the model.
% - resp: Column vector of size [nInst X 1] specifying the linear
%         multivariable model response when 'coeff' is applied to 'X'.
% - modelCI: Column vector of size [nInst X 2] specifying the 95%
%            confidence interval on the multivariable model response as
%            defined by the 2.5 (modelCI(i,1)) and the 97.5 (modelCI(i,2))
%            percentiles, for the ith instance. See ref. [1] for more
%            details.
% -------------------------------------------------------------------------
% AUTHOR(S):
% - Martin Vallieres <mart.vallieres@gmail.com>
% - DREES development team <http://www.cerr.info/drees> (logistic regression)
% - Francois Beauducel (roundsd.m)
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
% - Revision I: July 2015 (including imbalance-adjusted logistic regression)
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
% --> Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package 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 package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% --> Copyright (c) 2015, François Beauducel
%     All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
% -------------------------------------------------------------------------

% INITIALIZATION
warning off
nBoot = 1000;
alpha = 0.05;
bound = 1; % One standard error
order = size(X,2);
coeff = zeros(order,nBoot);
tol = 10^6; % To avoid extremely large coefficients
nInst = numel(Y);

% RANDOM NUMBER GENERATOR SEED
rng(seed);

% COMPUTING OVER ALL BOOTSTRAP SAMPLES
respBoot = zeros(size(X,1),nBoot);
modelCI = zeros(size(X,1),2);
for n = 1:nBoot
    average = inf;
    while average > tol
        bootSam = ceil(nInst .* rand(nInst,1));
        Xtrain = X(bootSam,:); Ytrain = Y(bootSam,1);

        coeff_temp = coxphfit(Xtrain,Ytrain,'censoring',censoring(bootSam),'baseline',0);

        coeff_temp(isnan(coeff_temp)) = 0;
        coeff(:,n) = coeff_temp;
        average = mean(abs(coeff(:,n)));
    end
    [respBoot(:,n)] = responseCox(X,coeff(:,n));
end
SE_coeff = bound.*(std(coeff')')./sqrt(nBoot);
coeff = mean(coeff')';

% CALCULATING THE BOOTSTRAP CONFIDENCE INTERVALS OF THE FINAL MODEL
for i = 1:size(X,1)
    modelCI(i,1)= prctile(respBoot(i,:),alpha/2*100);
    modelCI(i,2)= prctile(respBoot(i,:),(1-alpha/2)*100);
end

% ROUNDING COEFFICIENTS

% According to their standard errors, as in ref. [1]
% Note: This type of rounding seems too strong and may not be desirable, as it significantly affects the specificity of models
%       --> kept here as comments before further investigations
% for i = 1:order+1
%     if coeff(i) > 0
%         coeff(i) = roundsd(coeff(i),ceil(log10(abs(coeff(i)/roundsd(SE_coeff(i),1)))) + 1);
%     else
%         coeff(i) = roundsd(coeff(i),ceil(log10(abs(coeff(i)/roundsd(SE_coeff(i),1)))));
%     end
% end

% Rounding to 4 significant digits
% --> seems to preserve the predictive properties of models
coeff = roundsd(coeff,4);

% MULTIVARIABLE RESPONSE OF THE FINAL MODEL
[resp] = responseCox(X,coeff);
medianHR = median(resp);

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtRayTheatre.m                               |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Ray tracing with theatre                      |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
Xsrc  = [0 8 1.7];
Xmes  = [0 -20 8];
Nray  = 1e5
rad   = 1

% Read mesh
mesh        = msh('theatre.mesh');
mesh.col(:) = 100; % Rough concrete (Bobran, 1973)

% Extract toit
ctr = mesh.ctr;
mesh = mesh.sub(ctr(:,3)<14);

% Initialize ray
ray1 = ray(mesh,Xsrc,Nray);

% Graphical sphere
[X,Y,Z] = sphere(50);
X = Xmes(1) + rad*X; Y = Xmes(2) + rad*Y; Z = Xmes(3) + rad*Z;

% Graphical representation
figure
plot(mesh,'w')
hold on
plot(ray1)
surf(X,Y,Z,ones(size(X)))
axis equal
xlabel('X');   ylabel('Y');   zlabel('Z');
view(0,45)
alpha(0.5)

% Maximum distances
r1000 = rad/2 * sqrt(Nray/1000);
r100  = rad/2 * sqrt(Nray/100);
r10   = rad/2 * sqrt(Nray/10);
rMax  = rad/2 * sqrt(Nray/2);

% Ray-tracing
tic
ray1 = ray1.tracer(30,rMax);
toc
% plot(ray1)

% Images sources
tic
[img,nrg] = ray1.image(Xmes,rad,rMax);
toc

% Data
r   = sqrt(sum(img.^2,2));
sol = mean(nrg,2);

% Impacts
ray2 = ray(mesh,Xmes,img);
ray2 = ray2.tracer;
plot(ray2)
plot(msh(ray2.pos{2},(1:length(ray2))',10*log10(sol)));
axis equal
colorbar

% Energy in dB
figure
plot(r,10*log10(sol),'+b')
hold on
plot([r1000 r1000],[-60,0],'k--')
text(r1000,-55,' n = 1000')
plot([r100 r100],[-60,0],'k--')
text(r100,-57,' n = 100')
plot([r10 r10],[-60,0],'k--')
text(r10,-55,' n = 10')
plot([rMax rMax],[-60,0],'k--')
text(rMax,-55,' n = 1')
xlabel('Distance source mesure')
ylabel('Energie mesuree (dB)')
title('Energie mesuree selon la distance de mesure')
grid on

% Audio file
[audio,fs] = audioread('anechoicVoice.wav');

% Fir from images
T = floor(r/340*fs) + 1;
for i = 1:size(nrg,2)
    fir8(:,i) = accumarray(T,nrg(:,i),[max(T) 1]);
end

% Bank filtering
dir8 = ray1.bank(256,fs);
rir  = 0;
for i = 1:size(dir8,2)
    rir = rir + fftfilt(dir8(:,i),fir8(:,i));
end

% Graphical representation
figure
plot(rir)

% Audio rendering (5 seconds)
out = fftfilt(rir,audio(1:5*fs));
% sound(out,fs)

disp('~~> Michto gypsilab !')


function [caNodeIndices, vResolution_] = ex_CreateIndexCatalog(mCatalog, mPolygon, bMap, nGriddingMode, nNumberEvents, fRadius, fSizeRectHorizontal, fSizeRectDepth)
% function [caNodeIndices] = ex_CreateIndexCatalog(mCatalog, mPolygon, bMap, nGriddingMode,
%                                                  nNumberEvents, fRadius, fSizeRectHorizontal, fSizeRectDepth)
% -------------------------------------------------------------------------------------------------------------
% Creates a cell-array with subcatalogs for every grid node defined by mPolygon. These subcatalogs
%   contain only indices to the earthquake "rows" in mCatalog.
%
% Input parameters:
%   mCatalog              Earthquake catalog
%   mPolygon              Polygon (defined by ex_selectgrid)
%   bMap                  Calculate cell-array for a map (=1) or a cross-section (=0)
%   nGriddingMode         Mode of creating grid node subcatalogs
%                         0: Constant number of events
%                         1: Constant radius
%                         2: Rectangular grid node samples
%   nNumberEvents         Number of events per grid node (nGriddingMode == 0)
%   fRadius               Radius of grid node sample (nGriddingMode == 1)
%   fSizeRectHorizontal   Latitude/horizontal size of rectangle (nGriddingMode == 2)
%   fSizeRectDepth        Longitude/depth size of rectangle (nGriddingMode == 2)
%
% Output parameters:
%   caNodeIndices         Cell-array with index-catalogs per grid node of mPolygon
%
% Danijel Schorlemmer
% June 17, 2002

global bDebug;
if bDebug
  report_this_filefun(mfilename('fullpath'));
end

% Create the catalogs for each node with pointers to the overall catalog
nNumberNodes_ = length(mPolygon(:,1));
caNodeIndices = cell(nNumberNodes_, 1);

% If cross-section calculate the length along cross-section
if ~bMap
  [nRow_, nColumn_] = size(mCatalog);
  vXSecX_ = mCatalog(:,nColumn_);  % length along x-section
  vXSecY_ = (-1) * mCatalog(:,7);  % depth of hypocenters
end

% vResolution give the radius (for nGriddingMode = 0) and no. of events
% (for nGriddingMode = 1)
vResolution_(:,1)=ones(nNumberNodes_,1)*nan;

% Loop over all points of the polygon
for nNode_ = 1:nNumberNodes_
  % Get the grid node coordinates
  fX_ = mPolygon(nNode_, 1);
  fY_ = mPolygon(nNode_, 2);

  if (nGriddingMode == 0) | (nGriddingMode == 1)  % Fixed radius or fixed number
    % Calculate distance from center point
    if bMap
      vDistances_ = sqrt(((mCatalog(:,1)-fX_)*cos(pi/180*fY_)*111).^2 + ((mCatalog(:,2)-fY_)*111).^2);
    else
      vDistances_ = sqrt(((vXSecX_ - fX_)).^2 + ((vXSecY_ - fY_)).^2);
    end
    if nGriddingMode == 0 % Fixed number
      if length(mCatalog(:,1)) == 0
        caNodeIndices{nNode_} = [];
        % NaN for no events
        vResolution_(nNode_) = nan;
      elseif nNumberEvents > length(mCatalog(:,1))
        caNodeIndices{nNode_} = vIndices(1:length(mCatalog(:,1)));
        % take the maximal distance for all eq. in the catalog
        vResolution_(nNode_) = max(vdistances_);
      else
        % Use first nNumberEvents events
        [vTmp, vIndices] = sort(vDistances_);
        caNodeIndices{nNode_} = vIndices(1:nNumberEvents);
        % radius of the nNumberEvents-th event in the sorted vDistances_
        vResolution_(nNode_) = vTmp(nNumberEvents);
      end
    else % Fixed radius
      % Use all events within fRadius
      caNodeIndices{nNode_} = find(vDistances_ <= fRadius);
      vResolution_(nNode_) = length(find(vDistances_ <= fRadius));
    end
  else % Rectangular gridding (nGriddingMode == 2)
    if bMap
      vSel_ = ((mCatalog(:,1) >= (fX_ - fSizeRectHorizontal/2)) & (mCatalog(:,1) < (fX_ + fSizeRectHorizontal/2)) & ...
        (mCatalog(:,2) >= (fY_ - fSizeRectDepth/2)) & (mCatalog(:,2) < (fY_ + fSizeRectDepth/2)));
      vResolution_(nNode_) = length(find(vSel_ > 0))
    else
      vSel_ = ((vXSecX_ >= (fX_ - fSizeRectHorizontal/2)) & (vXSecX_ < (fX_ + fSizeRectHorizontal/2)) & ...
        (vXSecY_ >= (fY_ - fSizRectDepth/2)) & (vXSecY_ < (fY_ + fSizeRectDepth/2)));
      vResolution_(nNode_) = length(find(vSel_ > 0))
    end
    caNodeIndices{nNode_} = find(vSel_ == 1);
  end
end; % of for nNode_


%%******************************************************************
%% checkdepconst: compute AAt to determine if the
%%             constraint matrices Ak are linearly independent.
%%
%% [At,b,y,idxB,neardepconstr,feasible,AAt] =
%%         checkdepconstr(blk,At,b,y,rmdepconstr);
%%
%% rmdepconstr = 1, if want to remove dependent columns in At
%%             = 0, otherwise.
%%
%% idxB = indices of linearly independent columns of original At.
%% neardepconstr = 1 if there is nearly dependent columns in At
%%            = 0, otherwise.
%% Note: the definition of "nearly dependent" is dependent on the
%%       threshold used to determine the small diagonal elements in
%%       the LDLt factorization of A*At.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function  [At,b,y,idxB,neardepconstr,feasible,AAt] = ...
              checkdepconstr(blk,At,b,y,rmdepconstr);

   global existlowrank printlevel
   global nnzmatold
%%
%% compute AAt
%%
   m = length(b);
   AAt = sparse(m,m);  numdencol = 0; UU = [];
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if strcmp(pblk{1},'s');
         m1 = size(At{p,1},2); m2 = m - m1;
         if (m2 > 0)
            if (m2 > 0.3*m); AAt = full(AAt); end
            dd = At{p,3};
            lenn = size(dd,1);
            idxD = [0; find(diff(dd(:,1))); lenn];
            ss = [0, cumsum(pblk{3})];
            if (existlowrank)
               AAt(1:m1,1:m1) = AAt(1:m1,1:m1) + At{p,1}'*At{p,1};
               for k = 1:m2
                  idx = [ss(k)+1 : ss(k+1)];
                  idx2 = [idxD(k)+1: idxD(k+1)];
                  ii = dd(idx2,2)-ss(k); %% undo cumulative indexing
                  jj = dd(idx2,3)-ss(k);
                  len = pblk{3}(k);
                  Dk = spconvert([ii,jj,dd(idx2,4); len,len,0]);
                  tmp = svec(pblk,At{p,2}(:,idx)*Dk*At{p,2}(:,idx)');
                  tmp2 = AAt(1:m1,m1+k) + (tmp'*At{p,1})';
                  AAt(1:m1,m1+k) = tmp2;
                  AAt(m1+k,1:m1) = tmp2';
               end
            end
            DD = spconvert([dd(:,2:4); sum(pblk{3}),sum(pblk{3}),0]);
            VtVD = (At{p,2}'*At{p,2})*DD;
            VtVD2 = VtVD'.*VtVD;
            for k = 1:m2
               idx0 = [ss(k)+1 : ss(k+1)];
               %%tmp = VtVD(idx0,:)' .* VtVD(:,idx0);
               tmp = VtVD2(:,idx0);
               tmp = tmp*ones(length(idx0),1);
               tmp3 = AAt(m1+[1:m2],m1+k) + mexqops(pblk{3},tmp,ones(length(tmp),1),1);
               AAt(m1+[1:m2],m1+k) = tmp3;
            end
         else
            AAt = AAt + abs(At{p,1})'*abs(At{p,1});
         end
      else
         decolidx = checkdense(At{p,1}');
         if ~isempty(decolidx);
            n2 = size(At{p,1},1);
            dd = ones(n2,1);
            len= length(decolidx);
            dd(decolidx) = zeros(len,1);
            AAt = AAt + (abs(At{p,1})' *spdiags(dd,0,n2,n2)) *abs(At{p,1});
            tmp = At{p,1}(decolidx,:)';
            UU = [UU, tmp];
            numdencol = numdencol + len;
         else
            AAt = AAt + abs(At{p,1})'*abs(At{p,1});
         end
      end
   end
   if (numdencol > 0) & (printlevel)
      fprintf('\n number of dense column in A = %d',numdencol);
   end
   numdencol = size(UU,2);
%%
%%
%%
   feasible = 1; neardepconstr = 0;
   if ~issparse(AAt); AAt = sparse(AAt); end
   nnzmatold = mexnnz(AAt);
   rho = 1e-15;
   diagAAt = diag(AAt);
   mexschurfun(AAt,rho*max(diagAAt,1));
   [L.R,indef,L.perm] = chol(AAt,'vector');
   L.d = full(diag(L.R)).^2;
   if (indef)
      msg = 'AAt is not pos. def.';
      idxB = [1:m];
      neardepconstr = 1;
      if (printlevel); fprintf('\n checkdepconstr: %s',msg); end
      return;
   end
%%
%% find independent rows of A
%%
   dd = zeros(m,1);
   idxB = [1:m]';
   dd(L.perm) = abs(L.d);
   idxN = find(dd < 1e-13*mean(L.d));
   ddB = dd(setdiff([1:m],idxN));
   ddN = dd(idxN);
   if ~isempty(ddN) & ~isempty(ddB) & (min(ddB)/max(ddN) < 10)
      %% no clear separation of elements in dd
      %% do not label constraints as dependent
      idxN = [];
   end
   if ~isempty(idxN)
      neardepconstr = 1;
      if (printlevel)
         fprintf('\n number of nearly dependent constraints = %1.0d',length(idxN));
      end
      if (numdencol==0)
         if (rmdepconstr)
            idxB = setdiff([1:m]',idxN);
            if (printlevel)
               fprintf('\n checkdepconstr: removing dependent constraints...');
            end
            [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);
   	    tol = 1e-8;
            if (resnorm > sqrt(tol))
               idxB = [1:m]';
               neardepconstr = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: basis rows cannot be reliably identified,');
                  fprintf(' abort removing nearly dependent constraints');
               end
               return;
            end
            tmp = W'*b(idxB) - b(idxN);
            nnorm = norm(tmp)/max(1,norm(b));
            if (nnorm > tol)
               feasible = 0;
               if (printlevel)
                  fprintf('\n checkdepconstr: inconsistent constraints exist,');
                  fprintf(' problem is infeasible.');
               end
            else
               feasible = 1;
               for p = 1:size(blk,1)
                  At{p,1} = At{p,1}(:,idxB);
               end
               b = b(idxB);
               y = y(idxB);
               AAt = AAt(idxB,idxB);
            end
	 else
            if (printlevel)
               fprintf('\n To remove these constraints,');
               fprintf(' re-run sqlp.m with OPTIONS.rmdepconstr = 1.');
            end
         end
      else
         if (printlevel)
            fprintf('\n warning: the sparse part of AAt may be nearly singular.');
         end
      end
   end
%%*****************************************************************************
%% findcoeffsub:
%%
%% [W,resnorm] = findcoeffsub(blk,At,idXB,idXN);
%%
%% idXB = indices of independent columns of At.
%% idxN = indices of   dependent columns of At.
%%
%% AB = At(:,idxB); AN = At(:,idxN) = AB*W
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

   function [W,resnorm] = findcoeffsub(blk,At,idxB,idxN);

   AB = []; AN = [];
   for p = 1:size(blk,1)
      AB = [AB; At{p,1}(:,idxB)];
      AN = [AN; At{p,1}(:,idxN)];
   end
   [m,n] = size(AB);
%%
%%-----------------------------------------
%% find W so that AN = AB*W
%%-----------------------------------------
%%
   [L,U,P,Q] = lu(sparse(AB));
   rhs  = P*AN;
   Lhat = L(1:n,:);
   W = Q*( U \ (Lhat \ rhs(1:n,:)));
   resnorm = norm(AN-AB*W,'fro')/max(1,norm(AN,'fro'));
%%*****************************************************************************


function [estimate] = ft_inverse_harmony(sourcemodel, sens, headmodel, dat, varargin)

% FT_INVERSE_HARMONY computes a linear estimate of the current in a distributed
% source model using a mesh harmonic based low-pass filter.
%
% Use as
%   [estimate] = ft_inverse_harmony(sourcemodel, sens, headmodel, dat, ...)
% where
%   sourcemodel is the input source model, see FT_PREPARE_SOURCEMODEL
%   sens        is the gradiometer or electrode definition, see FT_DATATYPE_SENS
%   headmodel   is the volume conductor definition, see FT_PREPARE_HEADMODEL
%   dat         is the data matrix with the ERP or ERF
% and
%   estimate    contains the estimated source parameters
%
% Additional input arguments should be specified as key-value pairs and can include
%   'noisecov'             = Nchan x Nchan matrix with noise covariance
%   'noiselambda'          = scalar value, regularisation parameter for the noise covariance matrix (default=0)
%   'filter_order'         = scalar, order of the mesh Butterwirth filter
%   'filter_bs'            = scalar, stop-band of the mesh Butterworth filter
%   'number_harmonics'     = Integer, number of mesh harmonics used (can be empty, the default will then be identity)
%   'lambda'               = scalar, regularisation parameter (can be empty, it will then be estimated from snr)
%   'snr'                  = scalar, signal to noise ratio
%   'scalesourcecov'       = 'no' or 'yes', scale the source covariance matrix R such that trace(leadfield*R*leadfield')/trace(C)=1
%   'connected_components' = number of connected components of the source mesh (1 or 2)
%   'prewhiten'            = 'no' or 'yes', prewhiten the leadfield matrix with the noise covariance matrix C
%
% These options influence the forward computation of the leadfield
%   'reducerank'      = 'no' or number  (default = 3 for EEG, 2 for MEG)
%   'backproject'     = 'yes' or 'no', in the case of a rank reduction this parameter determines whether the result will be backprojected onto the original subspace (default = 'yes')
%   'normalize'       = 'no', 'yes' or 'column' (default = 'no')
%   'normalizeparam'  = parameter for depth normalization (default = 0.5)
%   'weight'          = number or Nx1 vector, weight for each dipole position to compensate for the size of the corresponding patch (default = 1)
%
% This implements
% - Petrov Y (2012) Harmony: EEG/MEG Linear Inverse Source Reconstruction in the
%   Anatomical Basis of Spherical Harmonics. PLoS ONE 7(10): e44439.
%   doi:10.1371/journal.pone.0044439
%
% See also FT_SOURCEANALYSIS, FT_PREPARE_HEADMODEL, FT_PREPARE_SOURCEMODEL

% TODO implement the following options
% - keepleadfield

% Copyright (C) 2015, Luca Ambrogio
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if mod(nargin-4,2)
  % the first 4 arguments are fixed, the other arguments should come in pairs
  ft_error('invalid number of optional arguments');
end

% get the optional input arguments, or use defaults
noisecov       = ft_getopt(varargin, 'noisecov');
lambda         = ft_getopt(varargin, 'lambda');  % can be empty, it will then be estimated based on SNR
noiselambda    = ft_getopt(varargin, 'noiselambda', []);
snr            = ft_getopt(varargin, 'snr');     % is used to estimate lambda if lambda is not specified
keepfilter     = istrue(ft_getopt(varargin, 'keepfilter', false));
dowhiten       = istrue(ft_getopt(varargin, 'prewhiten',  false));
doscale        = istrue(ft_getopt(varargin, 'scalesourcecov', false));

% construct the low-level options for the leadfield computation as key-value pairs, these are passed to FT_COMPUTE_LEADFIELD
leadfieldopt = {};
leadfieldopt = ft_setopt(leadfieldopt, 'reducerank',     ft_getopt(varargin, 'reducerank'));
leadfieldopt = ft_setopt(leadfieldopt, 'backproject',    ft_getopt(varargin, 'backproject'));
leadfieldopt = ft_setopt(leadfieldopt, 'normalize',      ft_getopt(varargin, 'normalize'));
leadfieldopt = ft_setopt(leadfieldopt, 'normalizeparam', ft_getopt(varargin, 'normalizeparam'));
leadfieldopt = ft_setopt(leadfieldopt, 'weight',         ft_getopt(varargin, 'weight'));

% flags to avoid calling isfield repeatedly in the loop over grid positions (saves a lot of time)
hasmom        = isfield(sourcemodel, 'mom');
hasleadfield  = isfield(sourcemodel, 'leadfield');
hasfilter     = isfield(sourcemodel, 'filter');

if isempty(lambda) && isempty(snr) && ~hasfilter
  ft_error('either lambda or snr should be specified');
elseif ~isempty(lambda) && ~isempty(snr)
  ft_error('either lambda or snr should be specified, not both');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find the dipole positions that are inside/outside the brain
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~isfield(sourcemodel, 'inside')
  if hasfilter
    sourcemodel.inside = ~cellfun(@isempty, sourcemodel.filter);
  elseif hasleadfield
    sourcemodel.inside = ~cellfun(@isempty, sourcemodel.leadfield);
  else
    sourcemodel.inside = ft_inside_headmodel(sourcemodel.pos, headmodel);
  end
end

% convert to logical representation
sourcemodel = fixinside(sourcemodel);

% keep the original details on inside and outside positions
originside = sourcemodel.inside;
origpos    = sourcemodel.pos;

% select only the dipole positions inside the brain for scanning
sourcemodel.pos    = sourcemodel.pos(originside,:);
sourcemodel.inside = true(size(sourcemodel.pos,1),1);

if hasmom
  sourcemodel.mom = sourcemodel.mom(:,originside);
end

if hasfilter
  ft_info('using precomputed filters\n');
  sourcemodel.filter = sourcemodel.filter(originside);
elseif hasleadfield
  ft_info('using precomputed leadfields\n');
  sourcemodel.leadfield = sourcemodel.leadfield(originside);
else
  ft_info('computing forward model on the fly\n');
  if hasmom
    for i=size(sourcemodel.pos,1)
      % compute the leadfield for a fixed dipole orientation
      sourcemodel.leadfield{i} = ft_compute_leadfield(sourcemodel.pos(i,:), sens, headmodel, leadfieldopt{:}) * sourcemodel.mom(:,i);
    end
  else
    for i=1:size(sourcemodel.pos,1)
      % compute the leadfield
      sourcemodel.leadfield{i} = ft_compute_leadfield(sourcemodel.pos(i,:), sens, headmodel, leadfieldopt{:});
    end
  end
end

% Harmony parameters
num_dip      = size(sourcemodel.leadfield{1}, 2);
num_pos      = size(sourcemodel.leadfield, 2);
num_comp     = ft_getopt(varargin, 'connected_components', 1);
num_harm     = ft_getopt(varargin, 'number_harmonics',     5);
filt_ord     = ft_getopt(varargin, 'filter_order',         5);
lc           = ft_getopt(varargin, 'filter_bs',            5);

% Create the leadfield matrix

% count the number of channels and leadfield components
Nchan   = size(sourcemodel.leadfield{1},1);
Nsource = 0;
for i=1:size(sourcemodel.pos,1)
  Nsource = Nsource + size(sourcemodel.leadfield{i}, 2);
end

% concatenate the leadfield components of all sources into one large matrix
lf = zeros(Nchan, Nsource);
n = 1;
for i=1:size(sourcemodel.pos,1)
  cbeg = n;
  cend = n + num_dip - 1;
  lf(:,cbeg:cend) = sourcemodel.leadfield{i};
  n = n + size(sourcemodel.leadfield{i}, 2);
end

% Create brain harmonics matrix

% Mesh harmonics
[dum,H,d] = mesh_spectrum(sourcemodel,num_harm,num_comp);

% Elimination of pathological zeroth frequencies and concatenating
% harmonics numbers
l = [];
for n = 1:num_comp
  d{n}(1) = 0;
  H{n}(:,1) = 1/sqrt(numel(H{n}(:,1)));
  d_cat = [];
  for m = 1:num_dip
    d_cat = [d_cat; d{n}];
  end
  l = [l; -d_cat];
end

% Total harmonics matrix
if num_comp == 1
  H_blk = H{1};
elseif num_comp == 2
  H_blk = blkdiag(H{1},H{2});
else
  ft_error('this function only supports meshes with 1 or 2 connected components')
end

if num_dip == 1
  H_tot = H_blk;
elseif num_dip == 2
  H_tot = blkdiag(H_blk,H_blk);
elseif num_dip == 3
  H_tot = blkdiag(H_blk,H_blk,H_blk);
else
  ft_error('this function only supports source model with 1, 2 or 3 dimensional source values')
end

% Create harmonic leadfield
lf_h = zeros(size(lf,1),num_harm,num_comp,num_dip); %leadifield in the harmonic domain

for k = 1:num_comp
  for j = 1:num_dip
    ind = (j-1) + 1:num_dip:num_dip*num_pos;
    lf_h(:,:,k,j) = lf(:,ind)*H{k};
  end
end

lf_h = reshape(lf_h,size(lf,1),num_comp*num_harm*num_dip);

% Create harmonic source covariance matrix
q = 1./sqrt(1 + (l/lc).^(2*filt_ord));
sourcecov = diag(q);

% Compute the Harmony spatial filters

% count the number of channels and leadfield components
Nchan   = size(sourcemodel.leadfield{1},1);
Nsource = 0;
for i=1:size(sourcemodel.pos,1)
  Nsource = Nsource + size(sourcemodel.leadfield{i}, 2);
end

% Take the rieal part of the noise cross-spectral density matrix
if isreal(noisecov)
  ft_info('taking the real part of the noise cross-spectral density matrix\n');
  noisecov = real(noisecov);
end

% compute the inverse of the forward model, this is where prior information
% on source and noise covariance would be useful
if isempty(noisecov)
  % use an unregularised minimum norm solution, i.e. using the Moore-Penrose pseudoinverse
  ft_warning('computing a unregularised minimum norm solution. This typically does not work due to numerical accuracy problems');
  w = pinv(lf_h);
elseif ~isempty(noisecov)
  ft_info('using the noise covariance for regularisation\n');
  % the noise covariance has been given and can be used to regularise the solution
  if isempty(sourcecov)
    sourcecov = speye(Nsource);
  end
  % rename some variables for consistency with the publications
  A = lf_h;
  R = sourcecov;
  C = noisecov;

  if dowhiten
    ft_info('prewhitening the leadfields using the noise covariance\n');

    % compute the prewhitening matrix
    if ~isempty(noiselambda)
      ft_info('using a regularized noise covariance matrix\n');
      % note: if different channel types are present, one should probably load the diagonal with channel-type specific stuff
      [U,S,V] = svd(C+eye(size(C))*noiselambda);
    else
      [U,S,V] = svd(C);
    end

    Tol     = 1e-12;
    diagS   = diag(S);
    sel     = find(diagS>Tol.*diagS(1));
    P       = diag(1./sqrt(diag(S(sel,sel))))*U(:,sel)'; % prewhitening matrix
    A       = P*A; % prewhitened leadfields
    C       = eye(size(P,1)); % prewhitened noise covariance matrix
  end

  if doscale
    % estimate sourcecov such that trace(ARA')/trace(C) = 1 (see
    % http://martinos.org/mne/manual/mne.html. In the case of prewhitening
    % C reduces to I (and then lambda^2 ~ 1/SNR); note that in mixed
    % channel type covariance matrices prewhitening should be applied in
    % order for this to make sense (otherwise the diagonal elements of C
    % have different units)
    ft_info('scaling the source covariance\n');
    scale = trace(A*(R*A'))/trace(C);
    R     = R./scale;
  end

  if ~isempty(snr)
    % the regularisation parameter can be estimated from the noise covariance,
    % see equation 6 in Lin et al. 2004
    lambda = trace(A * R * A')/(trace(C)*snr^2);
  end

  if dowhiten
    % as documented on MNE website, this is replacing the part of the code below, it gives
    % more stable results numerically.
    Rc      = chol(R, 'lower');
    [U,S,V] = svd(A * Rc, 'econ');
    s  = diag(S);
    ss = s ./ (s.^2 + lambda);
    w  = Rc * V * diag(ss) * U';

    % unwhiten the filters to bring them back into signal subspace
    w = w*P;

  else
    % equation 5 from Lin et al 2004 (this implements Dale et al 2000, and Liu et al. 2002)
    denom = (A*R*A'+(lambda^2)*C);
    if cond(denom)<1e12
      w = R * A' / denom;
    else
      ft_info('taking pseudo-inverse due to large condition number\n');
      w = R * A' * pinv(denom);
    end
  end

end % if empty noisecov

% for each of the timebins, estimate the source strength
w_x = H_tot*w; % The Harm matrix project the Harmony level filters back to the source locations

if isreal(dat)
  ft_info('the input consists of time-series: computing the dipole moments\n')
  mom = w_x * dat;
  mom_ind = 1;
elseif size(dat,1)==size(dat,2)&&sum(sum(dat-dat'))<10^-5*sum(diag(dat))
  ft_info('the input consists of a cross-spectral density: computing source-level power\n')
  pow_tot = real(sum((w_x*dat).*w_x,2));
  pow = 0;
  for j = 1:num_dip
    pow = pow + pow_tot((1 + (j-1)*end/num_dip):(j*end/num_dip));
    mom_ind = 0;
  end
else
  ft_info('the input consists of Fourier coefficients: computing source-level Fourier coefficients\n')
  mom = w_x*dat; % The Harm matrix project the Harmonic activity back to the source locations
  mom_ind = 1;
end

% assign the estimated source strength to each dipole
if mom_ind == 1
  n = 1;
  for i=1:size(sourcemodel.pos,1)
    cbeg = n;
    cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
    estimate.mom{i} = mom(cbeg:cend,:);
    n = n + size(sourcemodel.leadfield{i}, 2);
  end
end

% compute power (over the three orientations) at each location and for each time

if mom_ind == 1
  estimate.pow = nan(size(sourcemodel.pos,1), size(dat,2));
  for i=1:size(sourcemodel.pos,1)
    estimate.pow(i,:) = sum(abs(estimate.mom{i}).^2, 1);
  end
else
  estimate.pow = pow;
end

% deal with keepfilter option
if keepfilter && ~hasfilter
  % spatial filters have been computed, store them in the output
  % re-assign spatial filter to conventional 1 cell per dipole location
  n = 1;
  for i=1:size(sourcemodel.pos,1)
    cbeg = n;
    cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
    estimate.filter{i} = w(cbeg:cend,:);
    n    = n + size(sourcemodel.leadfield{i}, 2);
  end
elseif keepfilter
  estimate.filter = sourcemodel.filter;
end

% deal with noise covariance
% if ~isempty(noisecov) && ~hasfilter
%   % compute estimate of the projected noise
%   n = 1;
%   for i=1:size(sourcemodel.pos,1)
%     cbeg = n;
%     cend = n + size(sourcemodel.leadfield{i}, 2) - 1;
%     estimate.noisecov{i} = w(cbeg:cend,:)*noisecov*w(cbeg:cend,:)';
%     n    = n + size(sourcemodel.leadfield{i}, 2);
%   end
% elseif ~isempty(noisecov)
%   % compute estimate of the projected noise
%   for i=1:size(sourcemodel.pos,1)
%     estimate.noisecov{i} = sourcemodel.filter{i}*noisecov*sourcemodel.filter{i}';
%   end
%end

% reassign the estimated values over the inside and outside grid positions
estimate.inside  = originside;
estimate.pos     = origpos;
if isfield(estimate, 'pow')
  estimate.pow( originside,:) = estimate.pow;
  estimate.pow(~originside,:) = nan;
end
if isfield(estimate, 'mom')
  estimate.mom( originside) = estimate.mom;
  estimate.mom(~originside) = {[]};
end
if isfield(estimate, 'noisecov')
  estimate.noisecov( originside) = estimate.noisecov;
  estimate.noisecov(~originside) = {[]};
end
if isfield(estimate, 'filter')
  estimate.filter( originside) = estimate.filter;
  estimate.filter(~originside) = {[]};
end