function mfx = spm_cfg_mfx
% SPM Configuration file for MFX
%__________________________________________________________________________
% Copyright (C) 2010-2014 Wellcome Trust Centre for Neuroimaging

% Guillaume Flandin
% $Id: spm_cfg_mfx.m 6239 2014-10-13 14:53:48Z guillaume $

%--------------------------------------------------------------------------
% dir Directory
%--------------------------------------------------------------------------
dir         = cfg_files;
dir.tag     = 'dir';
dir.name    = 'Directory';
dir.help    = {'Select a directory where the SPM.mat file containing the specified design matrix will be written.'};
dir.filter = 'dir';
dir.ufilter = '.*';
dir.num     = [1 1];

%--------------------------------------------------------------------------
% spmmat Select SPM.mat
%--------------------------------------------------------------------------
spmmat         = cfg_files;
spmmat.tag     = 'spmmat';
spmmat.name    = 'Select SPM.mat files';
spmmat.help    = {...
    'Select the SPM.mat files that contains first-level designs.'
    'They must have the same number of parameters for each session.'
    ['These are assumed to represent session-specific realisations of ' ...
    '2nd-level effects.']};
spmmat.filter  = 'mat';
spmmat.ufilter = '^SPM\.mat$';
spmmat.num     = [1 Inf];

%--------------------------------------------------------------------------
% ffx Create first-level design
%--------------------------------------------------------------------------
ffx       = cfg_exbranch;
ffx.tag   = 'ffx';
ffx.name  = 'FFX Specification';
ffx.val   = {dir spmmat};
ffx.help  = {'Create FFX multi-session first-level design'};
ffx.prog  = @spm_local_ffx;
ffx.vout  = @vout_ffx;

%--------------------------------------------------------------------------
% spmmat Select SPM.mat
%--------------------------------------------------------------------------
spmmat         = cfg_files;
spmmat.tag     = 'spmmat';
spmmat.name    = 'Select SPM.mat';
spmmat.help    = {...
    'Design and estimation structure after a 1st-level analysis.'};
spmmat.filter  = 'mat';
spmmat.ufilter = '^SPM\.mat$';
spmmat.num     = [1 1];

%--------------------------------------------------------------------------
% con Contrast
%--------------------------------------------------------------------------
con         = cfg_entry;
con.tag     = 'contrast';
con.name    = 'Contrast';
con.help    = {
    'Contrast used to define 2nd level design matrix.'
    'E.g. ones(n,1) contrast where n is the number of sessions/subjects.'
    ['The specification of a contrast that is not ones(n,1) allows, ' ...
    'for example, specified sessions/subjects to be ignored.']
    ['If left empty (default), a contrast ones(n,1) will be specified ' ...
    'automatically at run time.']
    };
con.strtype = 'r';
con.num     = [Inf Inf];
con.val     = {[]};

%--------------------------------------------------------------------------
% spec MFX Specification
%--------------------------------------------------------------------------
spec       = cfg_exbranch;
spec.tag   = 'spec';
spec.name  = 'MFX Specification';
spec.val   = {spmmat con};
spec.help  = {'MFX Specification'};
spec.prog  = @spm_local_mfx;
spec.vout  = @vout_mfx;

%--------------------------------------------------------------------------
% mfx Mixed-effects (MFX) analysis
%--------------------------------------------------------------------------
mfx         = cfg_choice;
mfx.tag     = 'mfx';
mfx.name    = 'Mixed-effects (MFX) analysis';
mfx.help    = {'Mixed-effects (MFX) analysis'};
mfx.values  = {ffx spec};

%==========================================================================
% function out = spm_local_ffx(job)
%==========================================================================
function out = spm_local_ffx(job)
spmmat = job.spmmat;
SPMS = cell(size(spmmat));
for i=1:numel(spmmat)
    load(spmmat{i},'SPM');
    SPMS{i} = SPM;
end

matlabbatch{1}.spm.stats.fmri_spec.dir            = cellstr(job.dir);
matlabbatch{1}.spm.stats.fmri_spec.timing.units   = SPMS{1}.xBF.UNITS;
matlabbatch{1}.spm.stats.fmri_spec.timing.RT      = SPMS{1}.xY.RT;
matlabbatch{1}.spm.stats.fmri_spec.timing.fmri_t  = SPMS{1}.xBF.T;
matlabbatch{1}.spm.stats.fmri_spec.timing.fmri_t0 = SPMS{1}.xBF.T0;
switch SPMS{1}.xBF.name
    case 'hrf'
        matlabbatch{1}.spm.stats.fmri_spec.bases.hrf.derivs = [0 0];
    case 'hrf (with time derivative)'
        matlabbatch{1}.spm.stats.fmri_spec.bases.hrf.derivs = [1 0];
    case 'hrf (with time and dispersion derivatives)'
        matlabbatch{1}.spm.stats.fmri_spec.bases.hrf.derivs = [1 1];
    case 'Fourier set'
        matlabbatch{1}.spm.stats.fmri_spec.bases.fourier.length = SPMS{1}.xBF.length;
        matlabbatch{1}.spm.stats.fmri_spec.bases.fourier.order  = SPMS{1}.xBF.order;
    case 'Fourier set (Hanning)'
        matlabbatch{1}.spm.stats.fmri_spec.bases.fourier_han.length = SPMS{1}.xBF.length;
        matlabbatch{1}.spm.stats.fmri_spec.bases.fourier_han.order  = SPMS{1}.xBF.order;
    case 'Gamma functions'
        matlabbatch{1}.spm.stats.fmri_spec.bases.gamma.length = SPMS{1}.xBF.length;
        matlabbatch{1}.spm.stats.fmri_spec.bases.gamma.order  = SPMS{1}.xBF.order;
    case 'Finite Impulse Response'
        matlabbatch{1}.spm.stats.fmri_spec.bases.fir.length = SPMS{1}.xBF.length;
        matlabbatch{1}.spm.stats.fmri_spec.bases.fir.order  = SPMS{1}.xBF.order;
end
matlabbatch{1}.spm.stats.fmri_spec.volt   = SPMS{1}.xBF.Volterra;
matlabbatch{1}.spm.stats.fmri_spec.global = SPMS{1}.xGX.iGXcalc;
if isfield(SPMS{1}.xM,'gMT')
    matlabbatch{1}.spm.stats.fmri_spec.mthresh = SPMS{1}.xM.gMT;
end
if ~isempty(SPMS{1}.xM.VM)
    matlabbatch{1}.spm.stats.fmri_spec.mask = cellstr(SPMS{1}.xM.VM.fname); % can be intersection
end
if strncmp('AR',SPMS{1}.xVi.form,2)
    matlabbatch{1}.spm.stats.fmri_spec.cvi  = 'AR(1)';
elseif strcmp(SPMS{1}.xVi.form,'FAST')
    matlabbatch{1}.spm.stats.fmri_spec.cvi  = 'FAST';
else
    %matlabbatch{1}.spm.stats.fmri_spec.cvi  = 'none';
    disp('Forcing serial correlation modelling to be AR(1).');
    matlabbatch{1}.spm.stats.fmri_spec.cvi  = 'AR(1)';
end

k  = 1;
np = zeros(1,numel(SPMS));
disp(' ');
for i=1:numel(SPMS)
    fprintf('Subject %d:\n',i);
    np(i) = 0;
    n     = cumsum([1 SPMS{i}.nscan]);
    nSess = numel(SPMS{i}.Sess);
    for j=1:nSess
        fprintf(' Session %d:\n',j);
        matlabbatch{1}.spm.stats.fmri_spec.sess(k).scans = cellstr(SPMS{i}.xY.P(n(j):n(j+1)-1,:));
        nCond=numel(SPMS{i}.Sess(j).U);
        for l=1:nCond
            matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).name = SPMS{i}.Sess(j).U(l).name{1};
            matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).onset = SPMS{i}.Sess(j).U(l).ons;
            matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).duration = SPMS{i}.Sess(j).U(l).dur;
            o    = 1;
            nReg = numel(SPMS{i}.Sess(j).U(l).P);
            for m=1:nReg
                fprintf('  Condition %d: Columns %d\n',l,nReg);
                np(i) = np(i)+nReg;
                switch SPMS{i}.Sess(j).U(l).P(m).name
                    case 'time'
                        matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).tmod = SPMS{i}.Sess(j).U(l).P(m).h;
                    case 'none'
                    otherwise
                        matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).pmod(o).name  = SPMS{i}.Sess(j).U(l).P(m).name;
                        matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).pmod(o).param = SPMS{i}.Sess(j).U(l).P(m).P;
                        matlabbatch{1}.spm.stats.fmri_spec.sess(k).cond(l).pmod(o).poly  = SPMS{i}.Sess(j).U(l).P(m).h;
                        o = o + 1;
                end
            end
        end
        for l=1:numel(SPMS{i}.Sess(j).C.name)
            matlabbatch{1}.spm.stats.fmri_spec.sess(k).regress(l).name = SPMS{i}.Sess(j).C.name{l};
            matlabbatch{1}.spm.stats.fmri_spec.sess(k).regress(l).val  = SPMS{i}.Sess(j).C.C(:,l);
        end
        matlabbatch{1}.spm.stats.fmri_spec.sess(k).hpf = SPMS{i}.xX.K(j).HParam;
        k = k + 1;
    end
    disp(' ');
end

% Output number of sessions/conditions/columns for each subject

if any(np-np(1))
    disp('Error in FFX specification: all subject should have same number of parameters');
    return
end

spm_jobman('run',matlabbatch);

out.spmmat{1} = fullfile(job.dir{1},'SPM.mat');

%==========================================================================
% function dep = vout_ffx(job)
%==========================================================================
function dep = vout_ffx(job)
dep = cfg_dep;
dep.sname      = 'SPM.mat File';
dep.src_output = substruct('.','spmmat');
dep.tgt_spec   = cfg_findspec({{'filter','mat','strtype','e'}});

%==========================================================================
% function out = spm_local_mfx(job)
%==========================================================================
function out = spm_local_mfx(job)
load(job.spmmat{1},'SPM');
c = job.contrast;
if isempty(c)
    n = length(SPM.Sess);
    c = ones(n,1);
end
spm_mfx(SPM,c);
out.spmmat{1} = fullfile(fileparts(job.spmmat{1}),'mfx','SPM.mat');

%==========================================================================
% function dep = vout_mfx(job)
%==========================================================================
function dep = vout_mfx(job)
dep = cfg_dep;
dep.sname      = 'SPM.mat File';
dep.src_output = substruct('.','spmmat');
dep.tgt_spec   = cfg_findspec({{'filter','mat','strtype','e'}});


rows = 240;
cols = 360;
%fps = 1;
fps = 1/88800;
blk = 5;


function [stats, handles] = canlab_force_directed_graph(activationdata, varargin)
% Creates a force-directed graph from a set of variables, and plots
% clusters on 3-D brain as well if entered. Requires matlab BGL toolbox.
%
% :Usage:
% ::
%
%    canlab_force_directed_graph(activationdata OR connection matrix, ['cl', cl])
%
%
% ..
%     Author and copyright information:
%     -------------------------------------------------------------------------
%     Copyright (C) 2013  Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..
%
% :Inputs:
%
%   **activationdata:**
%        observations x variables matrix of data to be
%        inter-correlated (variables will be inter-correlated)
%
%        OR
%
%        signed, thresholded connection matrix to be used (e.g.,
%        average time-series correlation matrix across individuals
%        it's also possible to use thresholded t-values from multi-subject group analysis
%
%        If working with multi-subject time series data, best to enter connection matrix
%        as this function does not run models with participants as random
%        effects.
%
% :Optional Inputs: Enter keyword followed by variable with values
%
%   **'cl':**
%        followed by clusters or region structure with brain clusters
%
%   **'threshtype':**
%        followed by threshold type; 'bonf' [default] or 'fdr'
%
%   **'connectmetric':**
%        followed by node connection metric 'corr' or 'partial_corr'
%
%   **'sizescale':**
%        Followed by values to use in sizing of nodes on graph
%        'linear' 'sigmoidal' [default] or 'custom'
%
%   ** 'sizes' **
%       Followed by vector of point sizes for all points - works ONLY if sizescale
%       is set to 'custom'
%
%   **'rset', 'partitions':**
%        Cell of vectors, with indices (integers) of member
%        elements in each group, [1 x g] cell
%        rset can ALSO be a vector of integers, i.e., output
%        from clusterdata
%
%   **'setcolors', 'partitioncolors':**
%        Cell array of colors for each group, [1 x g]
%
%   **'names':**
%       followed by cell array of names for each region/object
%
%   **'namesfield':**
%       followed by name of field to extract region/object names from in
%       clusters structure
%
%   **'linewidth':**
%       followed by line width
%
%   **'linestyle':**
%       followed by line style. Default is 'curved', anything else is
%       'straight'
%
% :Output:
%
%   **stats:**
%        structure with descriptive statistics, including
%        betweenness-centrality, degree of each node
%
% :Examples:
% ::
%
%    [stats, handles] = canlab_force_directed_graph(activationdata, 'cl', cl, 'namesfield', 'shorttitle');
%    [stats, handles] = canlab_force_directed_graph(activationdata, 'cl', cl, 'namesfield', 'shorttitle', 'degree');
%    [stats, handles] = canlab_force_directed_graph(activationdata, 'cl', cl, 'namesfield', 'shorttitle', 'degree', 'rset', rset, 'setcolors', setcolors);
%
%    [stats, handles] = canlab_force_directed_graph(G, 'sizescale', 'custom', 'sizes', [6*ones(17, 1); 12*ones(7, 1)]);

% ..
%    DEFAULTS AND INPUTS
% ..

shan = [];              % outputs
spherehan = [];
handles = [];

cl = [];
threshtype = 'bonf';
connectmetric = 'corr';  % or partial_corr
sizescale = 'sigmoid';
sizes = ones(min(size(activationdata)), 1);
linestyle = 'curved';

setcolors = [];         % control of color subgroups
rset = [];

ptsizetype = 'bc';
names = [];
namesfield = [];        % enter field name, e.g., 'shorttitle'
linewidth = 1;

dofigure = true;

% Variable arg inputs
% -----------------------------------

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch lower(varargin{i})
            % reserved keywords
            case 'degree', ptsizetype = 'degree';
                %case 'design'

                % functional commands
            case 'cl', cl = varargin{i + 1}; varargin{i + 1} = [];

            case 'linewidth', linewidth = varargin{i + 1}; varargin{i + 1} = [];

            case 'linestyle', linestyle = varargin{i + 1}; varargin{i + 1} = [];

            case 'partitions', rset = varargin{i + 1}; varargin{i + 1} = [];

            case 'partitioncolors', setcolors = varargin{i + 1}; varargin{i + 1} = [];

            case {'threshtype' 'connectmetric' 'sizescale' 'setcolors' 'rset' 'names' 'namesfield' 'sizes' 'fdr'}
                eval([varargin{i} ' = varargin{i + 1}; varargin{i + 1} = [];'])

            case {'nofigure', 'nofig'}
                dofigure = false;

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

% Check vars, etc.
% ---------------------------------------------------------------------
% if isdir('/Users/tor/Documents/matlab_code_external/matlab_bgl')
%     g = genpath('/Users/tor/Documents/matlab_code_external/matlab_bgl');
%     addpath(g);
% end
%
% if ~exist('fruchterman_reingold_force_directed_layout.m', 'file')
%     error('Must have Matlab BGL toolbox on path (external toolbox)');
% end

activationdata = double(activationdata);

if ~isempty(cl) && (size(activationdata, 2) ~= length(cl))
    disp('activationdata must have as many columns as clusters has elements.')
    error('check activationdata and clusters to make sure they match.')
end

if isempty(rset)
    rset = {1:size(activationdata, 2)};
end

if isempty(setcolors)
    setcolors = scn_standard_colors(length(rset));
end

if ~isempty(namesfield)
    names = {cl.(namesfield)}';
end

if ~iscell(rset) % then integer vector
    for i = 1:max(rset)
        rset2{i} = find(rset == i);
    end
    rset = rset2;
end

% -------------------------------------------------------------------------
% CALCULATIONS
% -------------------------------------------------------------------------
if issymmetric(activationdata)
    fprintf('Thresholded association matrix detected.\n')
    [b, C, r, sig] = deal(activationdata);
    thr = NaN;

else
    fprintf('activationdata appears to be raw data. Running inter-correlations. See also xcorr_multisubject.\n')

     switch connectmetric
        case 'partial_corr'

            % Partial correlation slopes (r is actually slope)
            [r, p] = calc_partial_r(activationdata);

            b = r;

         case 'corr' % default

            [r, p] = corr(double(activationdata));

            b = [];

         otherwise error('Unknown connectmetric');
     end

     sz = size(r, 1);

     switch threshtype
         case 'bonf'
             thr = .05 ./ (sz * (sz - 1) / 2);  % bonferroni...

         case 'fdr'

             thr = p_matrix2fdrthresh(p);

     end

     C = r;

     % Threshold
     sig = sparse(double(p < thr & r > 0));
     signeg = sparse(-double(p < thr & r < 0));
     sig = sig + signeg;

     C(~sig) = 0;

     % Threshold based on significant partial regression effects
     % C is connectivity matrix for graph

    % end if issymmetric
end

% Enforce format for graph
C = (C + C') ./ 2;
C = sparse(C);

% Graph Stats
% -----------------------------------------------

% 2020: Use Matlab graph object
G = graph(C);

%bc = betweenness_centrality(abs(C));  % abs if ignoring neg connections
bc = centrality(G, 'betweenness');  % 'Importance', IMP

% shortest paths: used as estimate of connectivity
% [D S] = mean_path_length(r, rset);
% shortest path using POSITIVE edges only
GG = G;
GG.Edges.Weight( G.Edges.Weight < 0) = 0;

[D S] = mean_path_length(GG, rset);

deg = full(sum(C ~= 0))';

stats.names = names;
stats.rset = rset;
stats.C = C;
stats.r = r;
stats.b = b;

if exist('p', 'var')
    stats.p = p;
end

if exist('sig', 'var')
    stats.sig = sig;
end

stats.thr = thr;
stats.betweenness = bc;
%stats.path_length_distance = D;
stats.degree = deg;
%stats.mean_path_by_rset = S;

switch ptsizetype
    case 'bc'
        ptsizemetric = bc;
    case 'degree'
        ptsizemetric = deg;
    case 'none'
        ptsizemetric = ones(size(bc));
    otherwise error('Unknown ptsizetype');
end
if strcmp(sizescale,'custom')
   fprintf('Node size reflects custom scaling\n');
else
    fprintf('Node size reflects %s\n', ptsizetype);
end
% -------------------------------------------------------------------------
% Force-directed graph
% -------------------------------------------------------------------------

% Xc is coordinate matrix
% Xc = fruchterman_reingold_force_directed_layout(C);

han = plot(G, 'Layout', 'force');   % Get layout
Xc = [han.XData; han.YData]';
delete(han);                        % erase

% could rotate here
% ***

if dofigure
    if ~isempty(cl)
        create_figure('graph', 1, 2);
    else
        create_figure('graph');
    end
end

switch lower(linestyle)

    case 'curved'

        han = nmdsfig_tools('drawlines',Xc, sig, [.5 .5 .5; 0 1 1],[{'-'} {':'}], .1);
        handles.lh_pos = han.hhp;
        handles.lh_neg = han.hhn;% Dots
        %set(handles.lh_pos, 'Color', [.5 .5 .5])
        set(handles.lh_neg, 'Color', [0 .5 1], 'LineStyle', ':')

    otherwise

        gplot(sig>0, Xc, 'k');
        axis image
        lh = findobj(gca, 'Type', 'line');
        set(lh, 'Color', [.5 .5 .5])

        handles.lh_pos = lh;

        gplot(sig<0, Xc, 'b');
        lh = findobj(gca, 'Type', 'line', 'color', 'b');
        set(lh, 'Color', [0 .5 1], 'LineStyle', ':')

        handles.lh_neg = lh;

end

switch sizescale
    case 'linear'
        %         sizes = 4 + 15 * intensity.(cnames{i}) ./max(intensity.(cnames{i}));  % LC none of these variables exist in the workspace
        sizes = 4 + 15 * ptsizemetric ./ max(ptsizemetric);

    case 'sigmoid'
        % Rationale: avoids some VERY large points in areas highly
        % focused on as a priori ROIs, with extreme z-scores in
        % intensity relative to other areas.

        sizes = sigmoidscale(ptsizemetric);

    case 'custom'
        % we already have sizes
end

ph = [];
texth = {};

for k = 1:length(rset)

    texth{k} = [];

    for j = 1:length(rset{k})

        ph(end+1) = plot(Xc(rset{k}(j), 1), Xc(rset{k}(j), 2), 'o', 'Color', setcolors{k} ./ 2, 'MarkerSize', sizes(rset{k}(j)), 'MarkerFaceColor', setcolors{k}, 'LineWidth', linewidth);

        if ~isempty(names)
            offset = .02 * range(get(gca, 'XLim'));
            texth{k}(end+1) = text(Xc(rset{k}(j), 1)+offset, Xc(rset{k}(j), 2), strrep(names{rset{k}(j)}, '_', ' '), 'Color', 'k', 'FontSize', 14);
        end
    end

end

handles.ph = ph;
handles.texth = texth;

xlim = get(gca, 'XLim');  rg = range(xlim) * [-.05 .05]; xlim = xlim + rg;
ylim = get(gca, 'YLim'); rg = range(ylim) * [-.05 .05]; ylim = ylim + rg;
set(gca, 'XLim', xlim, 'YLim', ylim);
axis off
drawnow

stats.Xc = Xc;
stats.sizes = sizes;

if isempty(cl), return, end

% -------------------------------------------------------------------------
% Subcortical surface
% Only if cl is entered
% -------------------------------------------------------------------------

% NOTE: you need 3dHeadUtilityLite on your matlab path.
subplot(1, 2, 2)

% regioncenters
xyz = cat(1, cl.mm_center);

DB = struct('xyz', xyz, 'x', xyz(:, 1), 'y', xyz(:, 2), 'z', xyz(:, 3));

% sizes = sigmoidscale(ptsizemetric, 2, 6);
sizes = sizes ./ 2.5; % rescale to avoid huge ones

% Make Brain with Spheres
% ------------------------------------------------------------

[shan, spherehan] = connectivity3dbrain(xyz, rset, sizes, setcolors, names);

% Add lines
% ------------------------------------------------------------

[~, linehandles] = cluster_nmdsfig_glassbrain(cl,ones(length(cl), 1), {[.5 .5 .5]}, sig, [], 'samefig', 'nobrain', 'noblobs', linestyle);
set(linehandles, 'LineWidth', 2)

drawnow
%saveas(gcf, fullfile(savefigdir, ['graph3d_v2_' cnames{i} '.png']));

handles.surfhan = shan;
handles.spherehan = spherehan;
handles.linehandles = linehandles;

end % function

function [b p] = calc_partial_r(activationdata)

X = activationdata;
X = zscore(X);
%X(isnan(X)) = mean(X(~is;

clear b p

for i = 1:size(X, 2)
    y = X(:, i);
    xx = X;
    xx(:, i) = 1;  % intercept; need it, and also placeholder

    [bb, dev, statsglm] = glmfit(xx, y, 'normal', 'constant', 'off');

    b(:, i) = bb;
    p(:, i) = statsglm.p;
    p(i, i) = 1;
    b(i, i) = NaN;

end

end % function

function sizes = sigmoidscale(sizes, varargin)
% sizes = sigmoidscale(sizes, [lower bound], [upper bound])
%
% rescales a vector based on sigmoid function of zcore(input values)

% Rationale: avoids some VERY large points in areas highly
% focused on as a priori ROIs, with extreme z-scores in
% intensity relative to other areas.

% scale size of nodes - intensity, or betweenness
sizes = zscore(sizes);

A = 4; % lower asymptote
K = 15; % upper asymptote

if nargin > 2
    A = varargin{1};
    K = varargin{2};
end

B = 2;   % growth rate
v = .5;  % high-growth asymptote
Q = .5;
M = .5;  % time of max growth, if v = Q

richards = @(x, A, K, B, v, Q, M) A + (K - A) ./ ((1+Q*exp(-B*(x-M))).^(1/v));

%figure; plot(sort(sizes), richards(sort(sizes), A,K,B,v,Q,M));
sizes = richards(sizes, A,K,B,v,Q,M);

end

function [C S] = mean_path_length(G, rset)
% compute matrix of 1/path lengths for n x n matrix of regions, C (connectivity)
% and 1 / mean path length for sets of regions specified by rset
%
% r is a correlation matrix
% rset is a cell array of length k, for k sets, with vectors describing the
% indices of members of each set.
%
% e.g., r = region_r{i};

% 2020: Use Matlab Graph object
%
% r(isnan(r)) = 0;
% r = (r' + r) ./ 2;  % enforce symmetry, just in case
%
%
% % shortest paths: used to calculate connectivity
% rtmp = sparse(r);
% rtmp(rtmp < 0) = 0;

C = distances(G);

% OLDNote: Uses Floyd-Warshall method if 10% non-zero elements or more
% C = all_shortest_paths(rtmp);

% for each set, average elements of D corresponding to pairs of SETS
% return averages in S
%
% e.g., if D is min path length, S is the average min path length for Set 1
% vs. 2, 1 vs. 3, 2 vs. 3, etc.

% for inf values, we must impute some finite value or every average is Inf
% (i.e., if any regions are unconnected)
% impute max:
% but maybe better to work on unthresholded r matrix.
% Or, even better, return 1/S, 1 / mean path length, so unconnected
% regions/sets get 0.
%  mx = max(D(~isinf(D)));
%  D(isinf(D)) = mx;

S = zeros(length(rset));

for m = 1:length(rset)-1
    for n = m:length(rset)

        % set m to set n relationships (e.g., shortest path lengths)
        vals = 1 ./ C(rset{m}, rset{n});

        % average, excluding "self-connections" with value Inf
        S(m, n) = mean(vals(~isinf(vals)));

    end
end

S = S + S';

end

function [shan, spherehan] = connectivity3dbrain(regioncenters, rset, sizes, setcolors, names)

% Colors
% ----------------------------------------------------------

ctxcolors = cell(1, size(regioncenters, 1));

for i = 1:length(rset)

    for j = 1:length(rset{i})

        wh = rset{i}(j);

        ctxcolors(rset{i}) = setcolors(i);

    end
end

% Brain
% ----------------------------------------------------------

%create_figure('subcortex');

shan = addbrain('limbic');
delete(shan(end));
shan = shan(1:end-1);

shan = [shan addbrain('hires left')];
shan = [shan addbrain('brainstem')];

set(shan(end-1), 'FaceColor', [.5 .5 .5], 'FaceAlpha', .15);
set(shan(1:end-2), 'FaceColor', [.5 .5 .5], 'FaceAlpha', .3);
set(shan(end), 'FaceColor', [.5 .5 .5], 'FaceAlpha', .3);

view(89, 1);
lightRestoreSingle;

lighting gouraud;

% Spheres
% ----------------------------------------------------------

% add spheres
% cortex is special, because it involves different colors

% xyz = regioncenters(rset{1}, :);
% sz = sizes(rset{1});
% if ~isempty(names)
%     mynames = names(rset{1});
% end

xyz = regioncenters;
sz = sizes;
if ~isempty(names)
    mynames = names;
end

for i = 1:size(xyz, 1)
    spherehan(i) = cluster_image_sphere(xyz(i, :), 'color', ctxcolors{i}, 'radius', sz(i));

    if ~isempty(names)
        offset = .04 * range(get(gca, 'XLim'));
        text(xyz(i, 1)+offset, xyz(i, 2)+offset, xyz(i, 3)+offset, mynames{i}, 'Color', 'k', 'FontSize', 14);
    end
end
spherehan = {spherehan};

% for i = 2:length(rset)
%
%     xyz = regioncenters(rset{i}, :);
%     sz = sizes(rset{i});
%     if ~isempty(names)
%     mynames = names(rset{i});
%     end
%
%     spherehan{i} = cluster_image_sphere(xyz, 'color', setcolors{i}, 'radius', sz);
%
%     if ~isempty(names)
%         offset = .02 * range(get(gca, 'XLim'));
%         for j = 1:size(xyz, 1)
%             text(xyz(j, 1)+offset, xyz(j, 2)+offset, xyz(j, 3)+offset, mynames{j}, 'Color', 'k', 'FontSize', 14);
%         end
%     end
%
% end

end % function

function thr = p_matrix2fdrthresh(p)

trilp = tril(p, -1);
wh = logical(tril(ones(size(p)), -1)); % Select off-diagonal values (lower triangle)
trilp = double(trilp(wh));             % Vectorize and enforce double
trilp(trilp < 10*eps) = 10*eps;        % Avoid exactly zero values
fdrthr = FDR(trilp, 0.05);

if isempty(fdrthr), fdrthr = -Inf; end
thr = fdrthr;

end


function [surface_handles, colors] = isosurface(r, varargin)
% Create a series of surfaces in different colors, one for each region
% - Options for single color
%
% [surface_handles, colors] = isosurface(r, [optional arguments])
%
% optional arguments:
% - Any optional inputs to imageCluster, e.g., 'alpha'
% - 'colors', followed by single color in { } or cell array of multiple colors
% - 'nomatchleftright' or 'nosymmetric', do not match colors across hemispheres (left/right)
%               Note: The default matches, and may override your colors.
%
% Examples:
% atlasfile = which('Morel_thalamus_atlas_object.mat');
% load(atlasfile)
%
% surface_handles = isosurface(r);
% surface_handles = isosurface(r, 'alpha', .5);
% surface_handles = isosurface(r, 'alpha', .5, 'nomatchleftright');
%
% view(135, 30);
% lightRestoreSingle;
% lightFollowView;
%
% load(which('CIT168_MNI_subcortical_atlas_object.mat'));
% r = atlas2region(atlas_obj);
%
% surface_handles = isosurface(r, 'alpha', .5, 'color', {[.3 .6 .4] [.5 .4 .2]});
% surface_handles = isosurface(r, 'alpha', .5, 'color', {[.3 .6 .4] [.5 .4 .2]}, 'nomatchleftright');
% p = addbrain('hires right');
% lightFollowView;

k = length(r);

% ..
%    DEFAULTS AND INPUTS
% ..

% input_args = varargin;           % save for later
colors = scn_standard_colors(k); % generate colors
matchcolorsleftright = true;

% optional inputs with default values
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            % do nothing for inputs passed on to imageCluster
            case {'alpha'}

            case {'color', 'colors'}
                colors = varargin{i+1}; varargin{i+1} = []; varargin{i} = [];
                if length(colors) == 1 % single color; matchcolorsleftright will overwrite this
                    matchcolorsleftright = false;
                end

            case {'nomatchleftright', 'nosymmetric'}, matchcolorsleftright = false; varargin{i} = [];

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

if iscolumn(colors), colors = colors'; end

% Handle single-color input, and other cases where too few colors:
while length(colors) < k, colors = [colors colors]; end

if matchcolorsleftright
    colors = match_colors_left_right(r);
end

colors = colors(1:length(r));

% -------------------------------------------------------------------------
% Make surfaces
% -------------------------------------------------------------------------

cl = region2struct(r);

surface_handles = [];

for i = 1:k

    try
        out = imageCluster('cluster', cl(i), 'color', colors{i}, varargin{:});

        surface_handles(i) = out;
    catch
        disp('Error imaging isosurface; too few voxels?')
        %surface_handles(i) = [];
    end

%     % Isocaps, if needed
%
%     % pp = [];
%     isocap = isocaps(mesh_struct.X, mesh_struct.Y, mesh_struct.Z, V, mythresh);
%     p(end + 1) = patch(isocap, 'FaceColor', 'interp','EdgeColor', 'none', 'FaceAlpha',1);

end

% -------------------------------------------------------------------------
% Lighting, etc.
% -------------------------------------------------------------------------

lightRestoreSingle;
lighting gouraud;
axis vis3d image
material dull

end % function


%	OVERVIEW:
%       This is demo for the HRV PhysioNet Cardiovascular Signal Toolbox
%       using RR intervals with annotations.
%       Provided data are a subset from the MIT Physionet
%       NSR dataset, which contains long-term ECG recordings of subjects
%       in normal sinus rhythm.
%       It shows how to automaticly import multiple files from a folder,
%       perfrom the HRV analysis on each of them and then store the results
%       in .csv format.
%       It uses the default parameters in the configuration file using
%       'demo_NSR' option : InitializeHRVparams('demo_NSR').
%
%   OUTPUT:
%       HRV Metrics exported to .cvs files
%
%   DEPENDENCIES & LIBRARIES:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   REFERENCE:
%       Vest et al. "An Open Source Benchmarked HRV Toolbox for Cardiovascular
%       Waveform and Interval Analysis" Physiological Measurement (In Press), 2018.
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Giulia Da Poian
%	COPYRIGHT (C) 2018
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; clc; close all;

run(['..' filesep 'startup.m'])

% Remove old files generated by this demo
OldFolder = [pwd,filesep, 'OutputData', filesep, 'ResultsNSR'];
if exist(OldFolder, 'dir')
    rmdir(OldFolder, 's');
    fprintf('Old Demo Folder deleted \n \n');
end

% Initialize settings for demo
HRVparams = InitializeHRVparams('demo_NSR');
HRVparams.MSE.on = 0; % No MSE analysis for this demo
HRVparams.DFA.on = 0; % No DFA analysis for this demo
HRVparams.HRT.on = 0; % No HRT analysis for this demo
HRVparams.output.separate = 0;   % For this demo write all the results in one file

% Check for a list of files to be analyzed in current directory in .dat format
[subjectIDs,filesTBA] = GenerateListOfFilesTBA(HRVparams.ext, HRVparams.readdata,[]);

% Prepare for parallel loop by eliminating variables
clear nummatchingfiles x i filename flag match
numsub = length(subjectIDs);
notAnalyzed = 0;

% NOTE: This loop can be run in parallel by changing the loop to a parfor
% loop.
for i_patient = 1:numsub

    thisPatient = strcat(HRVparams.readdata, filesep, subjectIDs(i_patient));
    try

        % 1. Import Patient Data
        RRwindowStartIndices = [];
        tNN = [];
        NN = [];
        [samples,annotations] = read_ann(thisPatient{1},HRVparams.ext);
        rr = diff(samples)./HRVparams.Fs;
        t = cumsum(rr);

        % Demo keeps only the first 2h

        rr = rr(t<60*60*2);
        t = t(t<60*60*2);

        % 2. Perform HRV analysis on the RR intervals
        [results, resFilenameHRV] = Main_HRV_Analysis(rr,t,'RRIntervals',HRVparams,subjectIDs(i_patient),annotations);
        currentFile = [HRVparams.writedata filesep resFilenameHRV.HRV '.csv'];

        fprintf('\n');
    catch

        results = NaN;
        col_titles = {'NaN'};
        currentFile = '';
        notAnalyzed = 1;
        fprintf('Error on subject %s \n', char(subjectIDs(i_patient)));

    end

end

% 3. Compare generated output file with the reference one

referenceFile = ['ReferenceOutput' filesep 'NSR_HRV_allwindows_allpatients.csv'];
testHRV = CompareOutput(currentFile,referenceFile);

if testHRV
    fprintf('\n ** DemoAnnotatedData: TEST SUCCEEDED ** \n ')
elseif notAnalyzed == 0
    fprintf('\n ** DemoAnnotatedData: TEST FAILED ** \n')
    fprintf('Error: generated output does not match reference \n')
elseif notAnalyzed == 1
    fprintf('\n** DemoAnnotatedData: TEST FAILED ** \n')
    fprintf('Error: analysis not performed \n');
end


% Create new IM
newIndex = createNewIM();

% Compute dose for a beam
gantryAngle = 70;
isoCenter.x = 0;
isoCenter.y = -55;
isoCenter.z = -115;
newIndex = 2;
planC = batchCalcDose(newIndex,gantryAngle,isoCenter);


function varargout = process_arima( varargin )
% PROCESS_ARIMA: Auto-regressive Moving Average filter
%
% USAGE:   sProcess = process_arima('GetDescription')
%            sInput = process_arima('Run', sProcess, sInput, method=[])
%                 F = process_arima('Compute', F, Fbase, Order)

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

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'ARIMA filter';
    sProcess.FileTag     = 'arima';
    sProcess.Category    = 'Filter';
    sProcess.SubGroup    = 'Pre-process';
    sProcess.Index       = 68;
    sProcess.Description = '';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data', 'results', 'raw', 'matrix'};
    sProcess.OutputTypes = {'data', 'results', 'raw', 'matrix'};
    sProcess.nInputs     = 1;
    sProcess.nMinFiles   = 1;
    sProcess.processDim  = 1;   % Process channel by channel

    % Definition of the options
    % === Baseline time window
    sProcess.options.baseline.Comment = 'Baseline:';
    sProcess.options.baseline.Type    = 'baseline';
    sProcess.options.baseline.Value   = [];
    % === Filter order
    sProcess.options.order.Comment = 'Order of the filter:';
    sProcess.options.order.Type    = 'value';
    sProcess.options.order.Value   = {5,'',0};
    % === Sensor types
    sProcess.options.sensortypes.Comment = 'Sensor types or names (empty=all): ';
    sProcess.options.sensortypes.Type    = 'text';
    sProcess.options.sensortypes.Value   = 'MEG, EEG';
    sProcess.options.sensortypes.InputTypes = {'data', 'raw'};
end

%% ===== FORMAT COMMENT =====
function Comment = FormatComment(sProcess) %#ok<DEFNU>
    Order = sProcess.options.order.Value{1};
    if isfield(sProcess.options, 'baseline') && isfield(sProcess.options.baseline, 'Value') && iscell(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value{1})
        Time = sProcess.options.baseline.Value{1};
    else
        Time = [];
    end
    if isempty(Time)
        Comment = 'ARIMA(%d,1,0) - Baseline: [All file]';
    elseif any(abs(Time) > 2)
        Comment = sprintf('ARIMA(%d,1,0) - Baseline: [%1.3fs,%1.3fs]', Order, Time(1), Time(2));
    else
        Comment = sprintf('ARIMA(%d,1,0) - Baseline: [%dms,%dms]', Order, round(Time(1)*1000), round(Time(2)*1000));
    end
end

%% ===== RUN =====
function sInput = Run(sProcess, sInput) %#ok<DEFNU>
    % Get options
    Order = sProcess.options.order.Value{1};
    if isfield(sProcess.options, 'baseline') && isfield(sProcess.options.baseline, 'Value') && iscell(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value) && ~isempty(sProcess.options.baseline.Value{1})
        BaselineBounds = sProcess.options.baseline.Value{1};
    else
        BaselineBounds = [];
    end
    % Get baseline indices
    if ~isempty(BaselineBounds)
        iBaseline = panel_time('GetTimeIndices', sInput.TimeVector, BaselineBounds);
        if isempty(iBaseline)
            bst_report('Error', sProcess, [], 'Invalid baseline definition.');
            sInput = [];
            return;
        end
    % Get all file
    else
        iBaseline = 1:size(sInput.A,2);
    end
    % Filter data
    sInput.A = Compute(sInput.A, sInput.A(:,iBaseline), Order);
    % Error handling
    if isempty(sInput.A)
        bst_report('Error', sProcess, [], 'Error while filtering the signal.');
        sInput = [];
        return;
    end
    % Do not keep the Std field in the output
    if isfield(sInput, 'Std') && ~isempty(sInput.Std)
        sInput.Std = [];
    end
end

%% ===== EXTERNAL CALL =====
% USAGE: process_arima('Compute', F, Fbase=[], Order=5)
function F = Compute(F, Fbase, Order)
    % Default order: 5
    if (nargin < 3) || isempty(Order)
        Order = 5;
    end
    % If there is no baseline, use the whole time segment
    if (nargin < 2) || isempty(Fbase)
        Fbase = F;
    end
    Nsig = size(F,1);

    % If order is 0, just diff the signal
    if (Order == 0)
        % Detrend and diff data to filter
        F = diff(detrend(F'))';
        % Add one sample at the beginning to account for the diff
        F = [F(:,2), F];
    else
        % Detrend and diff baseline
        Fbase = diff(detrend(Fbase'));
        % Compute AR model for each signal
        if bst_get('UseSigProcToolbox')
            arm = lpc(Fbase, Order);
        else
            arm = zeros(Nsig, Order+1);
            for i = 1:Nsig
                arm(i,:) = oc_lpc(Fbase(:,i), Order);
            end
        end
        % Remove the ones for each it cannot be estimated
        arm(any(isnan(arm),2),:) = [];
        % If there is nothing left, error
        if isempty(arm)
            F = [];
        end
        % Average all the models
        arm = mean(arm,1);

        % Detrend and diff data to filter
        F = diff(detrend(F'));
        % Add a few samples of mirrored signal at the beginning to minimize the edge effects
        F = [F(length(arm)+1:-1:1,:); F];
        % Apply filter to the data
        F = filter(arm, 1, F)';
        % Remove the mirrored samples (keep one, to account for the diff)
        F(:,1:length(arm)) = [];
    end
end


function plotPredictionResults_STS(pathResults,fSetName,metrics,maxOrder)
% -------------------------------------------------------------------------
% function plotPredictionResults_STS(pathResults,fSetName,metrics,maxOrder)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function plots prediction performance estimation results for all the
% different feature set types entered as inputs. See ref. [1] for more
% details.
% -------------------------------------------------------------------------
% 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
% -------------------------------------------------------------------------
% INPUTS:
% - pathResults: Full path to the 'RESULTS' folder where prediction results
%                are saved.
% - fSetName: Cell of strings specifying the name of the type of feature
%                 sets to analyze.
%                 Example: fSetName = {'PET','SEPARATE','FUSED'}
% - metrics: Name of the metrics to show in the plots.
%            Example: metrics = {'AUC632','Sensitivity632','Specificity632'}
% - maxOrder: Integer specifying the maximal multivariable model order.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% 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
%
%    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/>.
% -------------------------------------------------------------------------

startpath = pwd;
cd(pathResults)

signs = {'-r',':b','--g'};
nFSET = numel(fSetName);
nMetrics = numel(metrics);
figure
for i = 1:nFSET
    val = zeros(maxOrder,numel(metrics));
    val_SE = zeros(maxOrder,nMetrics);
    results = load(['RESULTS_',fSetName{i},'_BEST']); results = struct2cell(results); results = results{1};
    for j = 1:maxOrder
        orderName = ['Order',num2str(j)];
        for k =1:nMetrics
            val(j,k) = results.(orderName).(metrics{k});
            val_SE(j,k) = results.(orderName).(['SE_',metrics{k}]);
        end
    end
    subplot(1,nFSET,i)
    for k = 1:nMetrics
        errorbar(1:maxOrder,val(:,k),val_SE(:,k),signs{k},'LineWidth',2,'MarkerFaceColor',signs{k}(end),'MarkerSize',4)
        hold on
    end
    xlabel('Model Order','FontSize',24)
    ylabel('Prediction performance','FontSize',24)
    title(fSetName{i},'FontSize',30,'FontWeight','bold')
    legend(metrics,'Location','SouthEast')
    axis([0 maxOrder+1 0.5 1])
    set(gca,'FontSize',20)
    set(gca,'XTick',[1 2 3 4 5 6 7 8 9 10])
    hold off
end

cd(startpath)
end


%-----------------------------------------------------------------------
% Job saved on 02-Jul-2014 11:27:33 by cfg_util (rev $Rev$)
% spm SPM - SPM12b (5672)
% cfg_basicio BasicIO - Unknown
%-----------------------------------------------------------------------
matlabbatch{1}.spm.spatial.smooth.data = '<UNDEFINED>';
matlabbatch{1}.spm.spatial.smooth.fwhm = [8 8 8];
matlabbatch{1}.spm.spatial.smooth.dtype = 0;
matlabbatch{1}.spm.spatial.smooth.im = 0;
matlabbatch{1}.spm.spatial.smooth.prefix = 's';


% PURPOSE: subroutine for ploterpGUI.m
%          identifies minimum and maximum values of ERP amplitudes for Y scaling.
%
% FORMAT
%
%  [yylim, serror] = erpAutoYLim(ERP, binArray, chanArray, xxlim)
%
% INPUTS
%
% ERP         - ERPset
% binArray    - indices of bins from where to get the amplitude values
% chanArray   - indices of channels from where to get the amplitude values
% xxlim       - current scale for time ([min max] in ms)
%
%
% OUTPUT
%
% yylim       - range for Y scale
% serror      - error flag. 0 means no errors.
%
% *** This function is part of ERPLAB Toolbox ***
% Author: Javier Lopez-Calderon & Steven Luck
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2013

function [yylim, serror] = erpAutoYLim(ERP, binArray, chanArray, xxlim,blcorrdata)
%ams updated blcorrdata
serror = 0;
if nargin<1
        error('erpAutoYLim needs 1 input argument at least.')
end

datatype = checkdatatype(ERP);

if nargin<5
    blcorrdata = 'no';
end

if nargin<4
        if strcmpi(datatype, 'ERP')
                xxlim = [ERP.xmin ERP.xmax]*1000;
        else
                xxlim = [ERP.xmin ERP.xmax];
        end
end
if nargin<3
        chanArray = 1:ERP.nchan;
end
if nargin<2
        binArray = 1:ERP.nbin;
end
if isempty(binArray)
        binArray = 1:ERP.nbin;
end
if isempty(chanArray)
        chanArray = 1:ERP.nchan;
end
if isempty(xxlim)
    if strcmpi(datatype, 'ERP')
        if strcmp(datatype,'CSD')
            datatype = 'ERP';     % if CSD data, treat as ERP data here
        end
        xxlim = [ERP.xmin ERP.xmax]*1000;
    else
        xxlim = [ERP.xmin ERP.xmax];
    end
end
try
        nbin  = length(binArray);
        nchan = length(chanArray);
        fs    = ERP.srate;

        if strcmpi(datatype,'CSD')
            datatype = 'ERP';     % if CSD data, treat as ERP data here
        end

        if strcmpi(datatype, 'ERP')

            if xxlim(1)<round(ERP.xmin*1000)
                aux_xxlim(1) = round(ERP.xmin*1000);
            else
                aux_xxlim(1) = xxlim(1);
            end
            if xxlim(2)>round(ERP.xmax*1000)
                aux_xxlim(2) = round(ERP.xmax*1000);
            else
                aux_xxlim(2) = xxlim(2);
            end
        else  % fft
            if xxlim(1)<5
                aux_xxlim(1) = 5; % to avoid including the spectrum under 5 Hz in calculating Y auto (too big!)
            else
                aux_xxlim(1) = xxlim(1);
            end
            if xxlim(2)>round(fs/2);
                aux_xxlim(2) = round(fs/2);
            else
                aux_xxlim(2) = xxlim(2);
            end
        end

        %disp('start')
        %aux_xxlim
        %[ERP.xmin ERP.xmax]
        %fs
        [p1, p2, checkw] = window2sample(ERP, aux_xxlim(1:2) , fs, 'relaxed');
        %disp('end')

        if strcmpi(blcorrdata,'no')
            datresh = reshape(ERP.bindata(chanArray,p1:p2,binArray), 1, (p2-p1+1)*nbin*nchan);
            yymax   = max(datresh);
            yymin   = min(datresh);
        else
            datresh = reshape(blcorrdata(chanArray,p1:p2,binArray), 1, (p2-p1+1)*nbin*nchan);
            yymax   = max(datresh);
            yymin   = min(datresh);
        end

        if abs(yymax)<1 && abs(yymin)<1
            yylim(1:2) = [yymin*1.2 yymax*1.1]; % JLC. Mar 11, 2015
        else
            yylim(1:2) = round([yymin*1.2 yymax*1.1]); % JLC. Sept 26, 2012
        end

        %
        % in case of flatlined ERPs
        %
        if yylim(1)<1E-6 && yylim(2)<1E-6
                if strcmpi(datatype, 'ERP')
                        yylim(1:2) = [-1 1];
                        fprintf('WARNING: It seems like erpAutoYLim() found flatlined ERPs. So auto Y-limit was set to [-1 1].\n');
                else
                        yylim(1:2) = [0 1];
                        fprintf('WARNING: It seems like erpAutoYLim() found flatlined Spectrum. So auto Y-limit was set to [0 1].\n');
                end
        end
catch
        if strcmpi(datatype, 'ERP')
                yylim(1:2) = [-10 10];
        else
                yylim(1:2) = [0 1];
        end
        serror =1;
        fprintf('WARNING: ERPLAB could not find the auto Y limits for %s.\nPlease check your input parameters and waverforms.\n', ERP.erpname);
        fprintf('Default Y limit values were loaded.\n');
        return
end


function [nav,stat]=decode_navb(nav,fid,opt,headinfo)
global glc gls
stat=1; NMAX=10000;
nav.eph=repmat(gls.eph,NMAX,1); nav.geph=repmat(gls.geph,NMAX,1);

switch headinfo.type
    case 'N',sys=headinfo.sys;
    case 'G',sys=glc.SYS_GLO;
    case 'L',sys=glc.SYS_GAL;
    case 'J',sys=glc.SYS_QZS;
    otherwise,stat=0;return;
end

while ~feof(fid)

    [eph,geph,type,fid,stat0]=decode_navb_data(opt,headinfo.ver,sys,fid);

    if stat0==1
        switch type
            case 1
                if nav.n+1>size(nav.eph,1)
                    nav.eph(nav.n+1:nav.n+NMAX,1)=repmat(gls.eph,NMAX,1);
                end
                nav.eph(nav.n+1)=eph;
                nav.n=nav.n+1;
            case 2
                if nav.ng+1>size(nav.geph,1)
                    nav.geph(nav.ng+1:nav.ng+NMAX,1)=repmat(gls.geph,NMAX,1);
                end
                nav.geph(nav.ng+1)=geph;
                nav.ng=nav.ng+1;
        end
    end
end
fclose(fid);

if nav.n<size(nav.eph,1)
    nav.eph(nav.n+1:end,:)=[];
end
if nav.ng<size(nav.geph,1)
    nav.geph(nav.ng+1:end,:)=[];
end

return


function [variables, index] = findGUBGroup(p,s)
variables = [];index = [];
for i = find((p.knapsack.type == 3) | (p.knapsack.type == 5))
    if all(ismember(s,p.knapsack.variables{i}))
        variables = p.knapsack.variables{i};
        index = i;
        return
    end
end


function [] = sb_fix_unwrap_manual(reset_flag,ix_reduced_list)
% [] = sb_fix_unwrap_manual(reset_flag)
% script to manually fix unwrapping errors in Small Baseline processing
%
% This function allows to manually fix unwrapping errors by iterating
% the results of ps_plot('rsb'). You can add add or subtract an integer number
% of 2pi cycles over a selected region, or correct with respect to the closest
% wrap of another region. You can keep iterating till the 'rsb'
% errors have reduced. If it goes wrong and you do want to reset your
% unwrapped data back to that of the original you can set the reset_flag to 1;
%
% By Bekaert David - University of Leeds - Sept 2014
%
% modifications:
% Bekaert David     05/2015     Fix in command line output, fix the code for ps_plot
% Bekaert David     09/2015     Add file restoring.
% Bekaert David     12/2016     Add closest wrap option, change to stamps_save code
% Bekaert David     12/2016     Add argument which is a subset of intergerograms to reduce plotting needs.
%

% deramp IFGs
deramp_flag =0;         % optional deramp rsb. Might be needed when processed with gamma
% what to use as guidence 'rsb' or 'usb' to click on
plot_option = 'usb';    % only 'rsb' or 'usb'
dem_option = 'd';    % only 'd' or ''

% flags
if nargin<1
    reset_flag=0;
end
if isempty(reset_flag)
    reset_flag=0;
end
if nargin<2
    ix_reduced_list=[];
end
if ~strcmpi(plot_option,'usb') && ~strcmpi(plot_option,'rsb')
    error('Only plot_option: rsb and usb supported');
end
if ~strcmpi(dem_option,'d') && ~isempty(dem_option)
    error('Only dem_option: d and [] supported');
end

% keeping the original data, incase user want to reset it
if exist('phuw_sb2_original.mat','file')~=2
    copyfile('phuw_sb2.mat','phuw_sb2_original.mat')
    copyfile('phuw2.mat','phuw2_original.mat')
    copyfile('phuw_sb_res2.mat','phuw_sb_res2_original.mat')
else
    copyfile('phuw_sb2.mat','phuw_sb2_previous_temp.mat')
    copyfile('phuw2.mat','phuw2_previous_temp.mat')
    copyfile('phuw_sb_res2.mat','phuw_sb_res2_previous_temp.mat')
end

% use previous or start from scratch
if reset_flag==1
   fprintf('Using the original data\n')
   ph_input = load('phuw_sb2_original');
else
   fprintf('Use data from a previous run\n')
   ph_input = load('phuw_sb2.mat');
end

% loading the interferogram information
ps = load('ps2.mat');

% generating list of ifgs to be ploted
drop_ifg_index = getparm('drop_ifg_index');
if ~isempty(ix_reduced_list)
    % removing ifgs that have been dropped before
    for k=1:length(drop_ifg_index)
        ix_temp = find(drop_ifg_index(k)==ix_reduced_list);
        ix_reduced_list(ix_temp)=[];
    end
    if sum(ix_reduced_list>ps.n_ifg)>0
        fprintf('Your list is larger than number of IFGS, will reset to max number of IFG \n')
        ix_reduced_list(ix_reduced_list>ps.n_ifg)=[];
    end
end
% reset the list in case nothing was left
if isempty(ix_reduced_list)
   ix_reduced_list = 1:ps.n_ifg;
   ix_reduced_list(drop_ifg_index)=[];
end

% deramping
if deramp_flag==1
    deramp_option = ['-o'];
else
    deramp_option = '';
end
if ~isempty(dem_option)
    if deramp_flag==1
        dem_option='-do';
    else
        dem_option= '-d';
    end
end
if strcmpi(plot_option,'usb')
    plot_option = [plot_option dem_option];
elseif strcmpi(plot_option,'rsb')
    plot_option = [plot_option deramp_option];
end

% plotting the current rsb data
ps_plot(['rsb' deramp_option],1,0,0,ix_reduced_list);

% get the interferogram that the user needs to adapt
repeat=1;
while repeat==1
    ix_ifg = input('Which interferogram to you want to correct? ','s');
    ix_ifg = str2num(ix_ifg);
    if isempty(ix_ifg)
        repeat=1;
    elseif ix_ifg<=ps.n_ifg
        repeat=0;
    else
        fprintf(['Not that many interferograms \n'])
    end
end

% plot the rsb value for this interferogram
% option one can use deramped rsb, this is for teh case teh interferograms
% were not created from relative differences, i.e. each interferogram had a
% baseline estimated and there might be some ramping errors because of that.
h_fig = ps_plot([plot_option],1,0,0,ix_ifg);
set(h_fig,'name',['Original interferogram']);

% Getting a polygon of the incorrect unwrapped area
fprintf('Define the incorrect unwrapped region through a polygon by clicking on the figure. \n')
repeat_zoom=1;
while repeat_zoom==1
    action_flag= input('First, zoom to your region of interest. Press [c] to continue. ','s');
    if strcmpi(action_flag,'c')
        repeat_zoom=0;
    end
end

fprintf('Now, start defining the polygon by outlining the incorrect region. \n Press enter once done\n')
% call the figure in case the user clicked somewhere else
figure(h_fig);
polygon=ginput;
% plotting the polygon on top
hold on
plot([polygon(:,1) ;polygon(1,1)],[polygon(:,2);polygon(1,2)],'r-','linewidth',2)

% loop untill the user is happy with it
continue_flag=1;
while continue_flag==1
    repeat=1;
    fprintf('You can shift the whole region by an integer number of cycles or you can put all pixels to a specifc wrap \n')
    while repeat==1
        ix_shift= input('By how many cycles to you want to shift this region? [+-integer or inf for wrap option] ','s');
        ix_shift = str2num(ix_shift);
        if isempty(ix_shift)
            repeat=1;
        elseif ix_shift==inf
            repeat=0;
        elseif (ix_shift./(round(ix_shift)))~=1
            fprintf(['Needs to be an integer number... \n'])
            repeat =1;
        else
            repeat=0;
        end
    end

    % finding the pixels within the polygon
    ix = inpolygon(ps.lonlat(:,1),ps.lonlat(:,2),polygon(:,1),polygon(:,2));

    % checking the option the user picked - closes wrap (inf) or shift region
    if ix_shift==inf            % closest wrap option
        fprintf('Define region to which you want to define as reference wrap (average will be used!). \n Press enter once done\n')
        % call the figure in case the user clicked somewhere else
        figure(h_fig);
        polygon_ref=ginput;
        % plotting the ploygon on top
        hold on
        plot([polygon_ref(:,1) ;polygon_ref(1,1)],[polygon_ref(:,2);polygon_ref(1,2)],'b-','linewidth',4)

        % finding the pixels within the reference polygon
        ix_ref = inpolygon(ps.lonlat(:,1),ps.lonlat(:,2),polygon_ref(:,1),polygon_ref(:,2));

        % check to which interferograms this should be applied
        repeat2=1;
        while repeat2==1
            action_flag= input('Do you want to apply this to all interferograms [y/n]? ','s');
            if strcmpi(action_flag,'y')
                repeat2=0;
            elseif strcmpi(action_flag,'n')
                repeat2=0;
            end
        end

        % store the orginal interferograms
        ix_ifg_or = ix_ifg;
        if strcmpi(action_flag,'y')
           ix_ifg=[1:size(ph_uw,2)];
        end

        % do the estimation for each itnerferogram
        ph_uw= ph_input.ph_uw;
        ref_phase = nanmean(ph_uw(ix_ref,ix_ifg),1);
        for k_ifgs=1:length(ix_ifg)
           % compute the reference
            radian_shift = round((ph_uw(ix,ix_ifg(k_ifgs))-ref_phase(k_ifgs))./(2*pi))*2*pi;
            ph_uw(ix,ix_ifg(k_ifgs)) = ph_uw(ix,ix_ifg(k_ifgs)) - radian_shift;
        end

        % update back to the previous interferogram to be corrected for
        % plotting purposes
        ix_ifg = ix_ifg_or;
        clear ref_phase

    else        % option of shifting the interferogram
        % the shift in radians
        radian_shift = ix_shift*2*pi;

        % modifying the interferogram
        ph_uw= ph_input.ph_uw;
        ph_uw(ix,ix_ifg)=ph_uw(ix,ix_ifg)+radian_shift;
    end
    msd = ph_input.msd;

    % saving the data
    stamps_save('phuw_sb2.mat',ph_uw,msd);

    % re-running the
    sb_invert_uw

    % plot the new residuals
    ps_plot(['rsb' deramp_option],1,0,0,ix_reduced_list);
    h_fig_new = ps_plot([plot_option],1,0,0,ix_ifg);
    set(h_fig_new,'name',['Corrected interferogram']);

    repeat=1;
    while repeat==1
        string= input('retry? [y/n] ','s');
        if strcmpi(string,'y')
            repeat=0;
            continue_flag = 1;
        elseif strcmpi(string,'n')
            repeat=0;
            continue_flag = 0;
            % see if the result needs to be kept or reverted
            repeat2=1;
            while repeat2==1
                action_flag= input('Keep this result [y/n]? ','s');
                if strcmpi(action_flag,'y')
                    repeat2=0;
                elseif strcmpi(action_flag,'n')
                    repeat2=0;

                    % restore the codes
                    copyfile('phuw_sb2_original.mat','phuw_sb2.mat')
                    copyfile('phuw2_original.mat','phuw2.mat')
                    copyfile('phuw_sb_res2_original.mat','phuw_sb_res2.mat')

                else
                    fprintf('y or n ...\n')
                end
            end
        else
            fprintf('y or n ...\n')
        end
    end

end


% This file is part of TREEQSM.
%
% TREEQSM 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.
%
% TREEQSM 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 TREEQSM.  If not, see <http://www.gnu.org/licenses/>.

function RS = relative_size(P,cover,segment)

% ---------------------------------------------------------------------
% RELATIVE_SIZE.M   Determines relative cover set size for points in new covers
%
% Version 2.00
% Latest update     16 Aug 2017
%
% Copyright (C) 2014-2017 Pasi Raumonen
% ---------------------------------------------------------------------
%
% Uses existing segmentation and its branching structure to determine
% relative size of the cover sets distributed over new covers. The idea is
% to decrease the relative size as the branch size decreases. This is
% realised so that the relative size at the base of a branch is
% proportional to the size of the stem's base, measured as number of
% cover sets in the first few layers. Also when we approach the
% tip of the branch, the relative size decreases to the minimum.
% Maximum relative size is 256 at the bottom of the
% stem and the minimum is 1 at the tip of every branch.
%
% Output:
% RS    Relative size (1-256), uint8-vector, (n_points x 1)

Bal = cover.ball;
Cen = cover.center;
Nei = cover.neighbor;
Segs = segment.segments;
SChi = segment.ChildSegment;
np = size(P,1);     % number of points
ns = size(Segs,1);  % number of segments

%% Use branching order and height as apriori info
% Determine the branch orders of the segments
Ord = zeros(ns,1);
C = SChi{1};
order = 0;
while ~isempty(C)
    order = order+order;
    Ord(C) = order;
    C = vertcat(SChi{C});
end
maxO = order+1; % maximum branching order (plus one)

% Determine tree height
Top = max(P(Cen,3));
Bot = min(P(Cen,3));
H = Top-Bot;

%% Determine "base size" compared to the stem base
% BaseSize is the relative size of the branch base compared to the stem
% base, measured as number of cover sets in the first layers of the cover
% sets. If it is larger than apriori upper limit based on branching order
% and branch height, then correct to the apriori limit
BaseSize = zeros(ns,1);
% Determine first the base size at the stem
S = Segs{1};
n = size(S,1);
if n >= 2
    m = min([6 n]);
    BaseSize(1) = mean(cellfun(@length,S(2:m)));
else
    BaseSize(1) = length(S{1});
end
% Then define base size for other segments
for i = 2:ns
    S = Segs{i};
    n = size(S,1);
    if n >= 2
        m = min([6 n]);
        BaseSize(i) = ceil(mean(cellfun(@length,S(2:m)))/BaseSize(1)*256);
    else
        BaseSize(i) = length(S{1})/BaseSize(1)*256;
    end
    bot = min(P(Cen(S{1}),3));
    h = bot-Bot; % height of the segment's base
    BS = ceil(256*(maxO-Ord(i))/maxO*(H-h)/H); % maximum apriori base size
    if BaseSize(i) > BS
        BaseSize(i) = BS;
    end
end
BaseSize(1) = 256;

%% Determine relative size for points
TS = 1;
RS = zeros(np,1,'uint8');
for i = 1:ns
    S = Segs{i};
    s = size(S,1);
    for j = 1:s
        Q = S{j};
        RS(vertcat(Bal{Q})) = BaseSize(i)-(BaseSize(i)-TS)*sqrt((j-1)/s);
    end
end

%% Adjust the relative size at the base of child segments
RS0 = RS;
for i = 1:ns
    C = SChi{i};
    n = length(C);
    if n > 0
        for j = 1:n
            S = Segs{C(j)};
            B = S{1};
            N = vertcat(Nei{B});
            if size(S,1) > 1
                N = setdiff(N,S{2});
            end
            N = union(N,B);
            N = vertcat(Bal{N});
            RS(N) = RS0(N)/2;
        end
    end
end


port = int16(2000);
client = py.carla.Client('localhost', port);
client.set_timeout(10.0);
world = client.get_world();

% Spawn Vehicle
blueprint_library = world.get_blueprint_library();
car_list = py.list(blueprint_library.filter("model3"));
car_bp = car_list{1};
spawn_point = py.random.choice(world.get_map().get_spawn_points());
tesla = world.spawn_actor(car_bp, spawn_point);
tesla.set_autopilot(true);

carla_is_running = true;

% Sensor 1
blueprint = world.get_blueprint_library().find('sensor.camera.rgb');
blueprint.set_attribute('image_size_x', '960')
blueprint.set_attribute('image_size_y', '540')
% blueprint.set_attribute('sensor_tick', '.1');
transform = py.carla.Transform(py.carla.Location(pyargs('x',-7.5, 'z',2.5)));
sensor = world.spawn_actor(blueprint, transform, pyargs('attach_to',tesla));

pyModule = sensorBind(sensor, "rgb", "rgb", "array");
currentImage = uint8(py.getattr(pyModule, 'array'));
imageHandle = imshow(currentImage);
set(gca,'units','pixels'); % set the axes units to pixels
x = get(gca,'position'); % get the position of the axes
set(gcf,'units','pixels'); % set the figure units to pixels
y = get(gcf,'position'); % get the figure position
set(gcf,'position',[y(1) y(2) x(3) x(4)]);% set the position of the figure to the length and width of the axes
set(gca,'units','normalized','position',[0 0 1 1]); % set the axes units to pixels
set(gcf,'menubar','none');
% Lidar
blueprint = world.get_blueprint_library().find('sensor.lidar.ray_cast');
blueprint.set_attribute('points_per_second', '56000');
blueprint.set_attribute('range', '5000');
blueprint.set_attribute('sensor_tick', '0.1');
transform = py.carla.Transform(py.carla.Location(pyargs('x',0.8, 'z',1.7)));
lidar = world.spawn_actor(blueprint, transform, pyargs('attach_to',tesla));

moduleLidar = sensorBind(lidar, 'lidar_file', 'lidar', 'array');

% Spawn NPC's
npc_bps = blueprint_library.filter("vehicle");
npc_to_spawn = 75;

% Preallocate memory
npc_list = cell(1, npc_to_spawn);

i = 1;
while i <= npc_to_spawn
    try
        npc_bp = py.random.choice(npc_bps);
        spawn_point = py.random.choice(world.get_map().get_spawn_points());
        npc_list{i} = world.spawn_actor(npc_bp, spawn_point);
        npc_list{i}.set_autopilot(true);
    catch
        % In case spawing fails due to collision, try again
        i = i - 1;
    end
    i = i + 1;
end

% A bounding box detector model.
detectorModel = HelperBoundingBoxDetector(...
    'XLimits',[-50 75],...              % min-max
    'YLimits',[-5 5],...                % min-max
    'ZLimits',[-2 5],...                % min-max
    'SegmentationMinDistance',1.6,...   % minimum Euclidian distance
    'MinDetectionsPerCluster',1,...     % minimum points per cluster
    'MeasurementNoise',eye(6),...       % measurement noise in detection report
    'GroundMaxDistance',0.3);           % maximum distance of ground points from ground plane

assignmentGate = [10 100]; % Assignment threshold;
confThreshold = [7 10];    % Confirmation threshold for history logic
delThreshold = [8 10];     % Deletion threshold for history logic
Kc = 1e-5;                 % False-alarm rate per unit volume

% IMM filter initialization function
filterInitFcn = @helperInitIMMFilter;

% A joint probabilistic data association tracker with IMM filter
tracker = trackerJPDA('FilterInitializationFcn',filterInitFcn,...
    'TrackLogic','History',...
    'AssignmentThreshold',assignmentGate,...
    'ClutterDensity',Kc,...
    'ConfirmationThreshold',confThreshold,...
    'DeletionThreshold',delThreshold,...
    'HasDetectableTrackIDsInput',true,...
    'InitializationThreshold',0);

% Create display
displayObject = HelperLidarExampleDisplay(uint8(py.getattr(pyModule, 'array')),...
    'PositionIndex',[1 3 6],...
    'VelocityIndex',[2 4 7],...
    'DimensionIndex',[9 10 11],...
    'YawIndex',8,...
    'MovieName','',...  % Specify a movie name to record a movie.
    'RecordGIF',false); % Specify true to record new GIFs

%% Loop Through Data
% Loop through the recorded lidar data, generate detections from the
% current point cloud using the detector model and then process the
% detections using the tracker.
start_time = cputime;

% Initiate all tracks.
allTracks = struct([]);

% Rotate point cloud
A = [0 -1 0 0; ...
     1  0 0 0; ...
     0  0 1 0; ...
     0  0 0 1];
tform = affine3d(A);

while carla_is_running
    try
        % Update time
        time = cputime - start_time;

        % Get current lidar scan
        XYZI = single(py.getattr(moduleLidar, 'array'));
        XYZ = lidarData(:, 1:3);

        % Flip the axis
        XYZ(:, 2) = -1 * XYZ(:, 2);
        currentLidar = pctransform(pointCloud(XYZ), tform);

        % Generator detections from lidar scan.
        [detections,obstacleIndices,groundIndices,croppedIndices] = detectorModel(currentLidar,time);

        % Calculate detectability of each track.
        detectableTracksInput = helperCalcDetectability(allTracks,[1 3 6]);

        % Pass detections to track.
        [confirmedTracks,tentativeTracks,allTracks] = tracker(detections,time,detectableTracksInput);

        % Get model probabilities from IMM filter of each track using
        % getTrackFilterProperties function of the tracker.
        modelProbs = zeros(2,numel(confirmedTracks));
        for k = 1:numel(confirmedTracks)
            c1 = getTrackFilterProperties(tracker,confirmedTracks(k).TrackID,'ModelProbabilities');
            modelProbs(:,k) = c1{1};
        end

        % Update display
        if isvalid(displayObject.PointCloudProcessingDisplay.ObstaclePlotter)
            % Get current image scan for reference image
            currentImage = uint8(py.getattr(pyModule, 'array'));

            set(imageHandle,'Cdata',currentImage);

            % Update display object
            displayObject(detections,confirmedTracks,currentLidar,obstacleIndices,...
                groundIndices,croppedIndices,[],modelProbs);
        end
    catch
        carla_is_running = false;
        close all;
    end
end

tesla.destroy();
sensor.destroy();
lidar.destroy();

for i=1:npc_to_spawn
   npc_list{i}.destroy();
end


function h = specgram2(s, T, varargin)
   %SPECGRAM2 - plots spectrograms of waveforms with waveform along top
   %  h = specgram2(spectralobject, waveforms) generates a spectrogram from
   %  the waveform(s), overwriting the current figure.  The waveform will be
   %  displayed along the top of the spectrogram. The return value is a handle
   %  to the spectrogram, and is optional.
   %
   %  The spectrograms will be created in the same shape as the passed
   %  waveforms.  ie, if W is a 2x3 matrix of waveforms, then
   %  specgram2(spectralobject,W) will generate a 2x3 plot of spectra.
   %
   %  Many additional behaviors can be modified through the passing of
   %  additional parameters, as listed further below.  These parameters are
   %  always passed in pairs, as such:
   %
   %  specgram2(spectralobject, waveforms,'PARAM1',VALUE1,...,'PARAMn',VALUEn)
   %    Any number of these parameters may be passed to specgram2.
   %
   %  specgram2(..., 'axis', AXIS_HANDLE)
   %    Specify the axis AXIS_HANDLE within which the spectrogram will be
   %    generated.  The boundary of the axis becomes the boundary for the
   %    entire spectra plot.  For a matrix of waveforms, this area is
   %    subdivided into NxM subplots, where N and M are the size of the
   %    waveform matrix.
   %
   %  specgram2(..., 'xunit', XUNIT)
   %    Spedifies the x-unit scale to be used with the spectrogram.  The
   %    default unit is 'seconds'.
   %    valid xunits:
   %     'seconds','minutes','hours','days','doy' (day of year),and 'date'
   %
   %  specgram2(..., 'colormap', ALTERNATEMAP)
   %    Instead of using the default colormap, any colormap may be used.  An
   %    alternate way of setting the global map is by using the SETMAP
   %    function.  ALTERNATEMAP will either be a name (eg. grayscale) or an
   %    Nx3 numeric. Type HELP GRAPH3D to see additional useful colormaps.
   %
   %
   %  specgram2(..., 'colorbar', COLORBAR_OPTION)
   %    Generates a spectrogram from the waveform and uses a specific map
   %    valid COLORBAR_OPTION values: 'horiz' (default),'vert','none',
   %      'HORIZ' places a single colorbar below all plots
   %      'VERT' places a single colorbar to the right of all plots
   %      'NONE' supresses the colorbar placement
   %
   %  specgram2(..., 'yscale', YSCALE)
   %    Choosing 'log' Allows the y-axis to be generated on a log-frequency
   %    scale, with uneven vertical cell spacing.  The default value is
   %    'normal', and provides the standard spectrogram view.
   %    valid yscales: 'normal', 'log' (see NOTE below)
   %
   %    NOTE: In order to use the log scale, UIMAGESC needs to be available on
   %    the matlab path.  This routine was created by Frederic Moisy, and may
   %    be downloaded from the maltabcentral fileexchange (File ID: 11368).
   %    If this routine is not found,then the original spectrogram will be
   %    created.
   %
   %  specgram2(..., 'fontsize', FONTSIZE)
   %    Specify the font size for a spectrogram.  The default font size is 8.
   %
   %  specgram2(..., 'innerLabels', SHOWINNERLABELS)
   %    Suppress the labling of the inside graphs by setting SHOWINNERLABELS
   %    to false.  If this is false, then the frequency label only shows on
   %    the leftmost spectrograms, and the X-unit label only shows on the
   %    bottommost spectrograms.
   %
   %  Example 1:
   %    % The following plots a waveform using an alternate mapping, an xunit of
   %    % of 'hours', and with the y-axis plotted using a log scale.
   %    specgram2(spectralobject, waveform,...
   %      'colormap', alternateMap,'xunit','h','yscale','log')
   %
   %
   %  Example 2:
   %    % create an arbitrary subplot, and then plot multiple spectra
   %    a = subplot(3,2,1);
   %    specgram2(spectralobject,waves,'axis',a); % waves is an NxM waveform
   %
   %   See also SPECTRALOBJECT/SPECGRAM

   if ~isa(T,'TraceData')
            try
               T = SeismicTrace(T);
               disp('successfully converted to a SeismicTrace');
            catch er
      error('Spectralobject:specgram2:invalidArgument','Should work on a trace (ex. TraceData, SeismicTrace), not a %s',class(T));
            end
   end

   %% search for relevent property pairs passed as parameters
   p = parseSpecgramInputs(s, varargin);

   %% figure out exactly WHERE to plot the spectrogram(s)
   %find out area(axis) in which the spectrograms will be plotted
   clabel= 'Relative Amplitude  (dB)';

   if p.Results.axis == 0,
      clf;
      pos = get(gca,'position');
   else
      pos = get(p.Results.axis,'position');
   end
   if ~isempty(p.Results.position)
      pos = p.Results.position;
   end

   %% If there are multiple waveforms...
   % subdivide the axis and loop through specgram2 with individual waveforms.
   if numel(T) > 1
      if p.Results.axis== 0,
         myaxis = gca;
      end
      %create the colorbar if desired
      TraceSpectra.createcolorbar(s,p.Results.colorbar, clabel, p.Results.fontsize);
      h = TraceSpectra.subdivide_axes(myaxis, size(T));
      % remainingproperties = TraceSpectra.property2varargin(proplist);
      remainingproperties = TraceSpectra.buildParameterList( p.Unmatched);
      for n=1:numel(h)
         keepYlabel =  ~p.Results.innerlabels || (n <= size(h,1));
         keepXlabel = ~p.Results.innerlabels || (mod(n,size(h,2))==0);
         specgram2(s,T(n),...
            'xunit',p.Results.xunit,...
            'axis',h(n),...
            'fontsize',p.Results.fontsize,...
            'useXlabel',keepXlabel,...
            'useYlabel',keepYlabel,...
            'colorbar','none',...
            remainingproperties{:});
      end
      return
   end

   %% Plot the spectrogram with a wiggle on top and colorbar below

   %plot the wiggle
   ax_wiggle = subplot('position',wigglePosition(pos));
   plot(T,'xunit',p.Results.xunit,'autoscale',true,'fontsize',p.Results.fontsize);

   % make the axis tight, and keep axis info for later use with spectra
   axis(ax_wiggle,'tight');
   xAxisLims = get(ax_wiggle,'xlim');
   ticnos = get(ax_wiggle,'xtick');

   %plot the spectra
   ax_spectra = subplot('position',spectraPosition(pos));
   additionalParams = TraceSpectra.buildParameterList( p.Unmatched);
   specgram(s,T,...
      'xunit',p.Results.xunit,...
      'fontsize',p.Results.fontsize,...
      'yscale',p.Results.yscale,...
      'colorbar','none',...
      'axis',ax_spectra,...
      'suppressXlabel',p.Results.useXlabel,...
      'suppressYlabel',p.Results.useYlabel,...
      additionalParams{:});

   %make the axis match exactly with the waveform above
   set(ax_spectra,'xtick',ticnos);
   if ~strcmpi(p.Results.yscale,'log')
      % axis scaling doesn't work quite right at a log scale
      xlim(ax_spectra, xAxisLims);
   end

   title(''); %clear the title

   %create the colorbar if desired
   TraceSpectra.createcolorbar(s,p.Results.colorbar, clabel, p.Results.fontsize);
end

function wigPos = wigglePosition(pos)
   % wigglePosition   wiggle occupies the top 15% of the axis
   % pos = [ left, bottom, width, height ]
   % wavepos = [ left, bottom + height * 0.85,  width , height * 0.15] ;
   wigPos = pos .* [1, 1, 1, 0.15] + [0,  pos(4) * 0.85, 0, 0] ;
end

function specPos = spectraPosition(pos)
   %spectraPosition   spectra occupies the bottom 85% of the axis
   specPos = pos .* [1, 1, 1, 0.85 ];
end


function [] = SLC_export(slcstack,slclist, Path, extention, InSAR_processor, reference_index)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   This file is part of TomoSAR.
%
%   TomoSAR is distributed in the hope that it will be useful,
%   but without warranty of any kind; without even the implied
%   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
%   See the Apache License for more details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Author : Dinh Ho Tong Minh (INRAE) and Yen Nhi Ngo, Jan. 2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if not(exist('InSAR_processor', 'var'))
     InSAR_processor = 'snap';
end

if not(exist('reference_index', 'var'))
     reference_index = 1;
end

[nlines,nwidths,n_slc] = size(slcstack);

real_index = 1:2:nwidths*2-1;
imag_index = 2:2:nwidths*2;
line_cpx = zeros(2*nwidths, 1);

for i = 1:n_slc
     switch InSAR_processor
        case 'snap' %
            filename = [Path,'/',num2str(slclist(i)),extention];
            fid = fopen(filename, 'wb', 'ieee-be');
        case 'isce'
            if i == reference_index
                filename = [Path,'/reference/','reference.slc',extention];
            else
                filename = [Path,'/',num2str(slclist(i)),'/','secondary.slc',extention];
            end
            fid = fopen(filename, 'wb');
        otherwise
            disp('not yet support')
    end

    data = squeeze(slcstack(:,:,i));
    for k=1:nlines
        line_cpx(real_index) = real(data(k,:));
        line_cpx(imag_index) = imag(data(k,:));
        line_count = fwrite(fid, line_cpx, 'float32');
    end
    fclose(fid);
end


% SP_TO_VTK: Export multipatch results to VTK format for plotting.
%
%  sp_to_vtk (u, space, geometry, npts, filename, fieldnames, [option], [lambda_lame, mu_lame])
%
% INPUT:
%
%     u:           vector of dof weights
%     space:       object representing the space of discrete functions (see sp_multipatch)
%     geometry:    geometry structure (see geo_load)
%     npts:        number of points along each parametric direction where to evaluate
%     filename:    name of the output file.
%     fieldnames:  how to name the saved variables in the vtk file
%     options:     cell array with the fields to plot
%                   accepted options are 'value' (default), 'gradient',
%                   and for vectors also 'curl', 'divergence', 'stress'
%     lambda_lame: function handle to the first Lame coefficient (only needed to compute 'stress')
%     mu_lame:     function handle for the second Lame coefficient (only needed to compute 'stress')
%
% OUTPUT:
%
%    none
%
% Copyright (C) 2010 Carlo de Falco, Rafael Vazquez
% Copyright (C) 2011, 2012, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp_to_vtk (u, space, geometry, npts, filename, fieldname, varargin)

  str1 = cat (2,'<?xml version="1.0"?> \n', ...
'<VTKFile type="Collection" version="0.1"> \n', ...
'<Collection> \n');

  str2 = cat (2, '<DataSet part="%d" file="%s.vts"/> \n');

  str3 = cat (2, ...
'</Collection>\n', ...
'</VTKFile> \n');

  if (length (filename) < 4 || ~strcmp (filename(end-3:end), '.pvd'))
    pvd_filename = cat (2, filename, '.pvd');
  else
    pvd_filename = filename;
    filename = filename (1:end-4);
  end

  fid = fopen (pvd_filename, 'w');
  if (fid < 0)
    error ('mp_sp_to_vtk: could not open file %s', pvd_filename);
  end

  fprintf (fid, str1);
  ind = union (find (filename == '/', 1, 'last'), find (filename == '\', 1, 'last')) + 1;
  if (isempty (ind)); ind = 1; end
  for iptc = 1:space.npatch
    filename_patch_without_path = cat (2, filename(ind:end), '_', num2str (iptc));
    filename_patch = cat (2, filename, '_', num2str (iptc));
    fprintf (fid, str2, iptc, filename_patch_without_path);
    if (isempty (space.dofs_ornt))
      sp_to_vtk (u(space.gnum{iptc}), space.sp_patch{iptc}, geometry(iptc), npts, ...
                           filename_patch, fieldname, varargin{:})
    else
      sp_to_vtk (u(space.gnum{iptc}) .* space.dofs_ornt{iptc}', space.sp_patch{iptc}, geometry(iptc), npts, ...
                           filename_patch, fieldname, varargin{:})
    end
  end
  fprintf (fid, str3);

  fclose (fid);

end


function [p,q] = detectAndMatchFeatures(image1,image1ROI,image2,image2ROI,featureOpts)
%detectAndMatchFeatures: A wrapper for detecting and matching features
%between two images using various feature detection methods.

%Inputs
%
%image1: The first image (2D unit8, grayscale or rgb).
%
%image1ROI: A region-of-interest specifying which region(s) in image1 we
%should detect features in. If image1ROI=[] then the whole image is used.
%
%image2: The second image (2D unit8, grayscale or rgb).
%
%image2ROI: A region-of-interest specifying which region(s) in image2 we
%should detect features in. If image2ROI=[] then the whole image is used.
%
%featureOpts: Options field specifying the feature detection method.
%Currently only SURF (matlab's built-in) and ASIFT
%(http://www.ipol.im/pub/art/2011/my-asift/) are supported, but it is very
%easy to introduce others.
%for ASIFT, featureOpts should have the structures:
%
%   featureOpts.featureMethod = 'ASIFT';
%   featureOpts.asiftPath (the path to the asift code & compiled executable).
%
%for SURF, featureOpts should have the structures:
%
%   featureOpts.featureMethod = 'SURF'; %careful, unlike ASIFT this only works well
%when the plane's viewpoint is not too tilted!!
%
%   featureOpts.loweRatioThreshold = 1.2 (default) You need to
%set a confidence ratio (see Lowe's SIFT paper for the explanation of
%this). Basically, a high value means only using feature matches that are
%likely to be correct (but at the cost of fewer feature matches). A default
%of 1.2 is usually fine.
%
% outputs:
%p : 2XN matrix holding the matched points in image1
%q : 2XN matrix holding the matched points in image2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of the IPPE package for fast plane-based pose
% estimation from the paper "Infinitesimal Plane-based Pose Estimation" by Toby Collins and Adrien Bartoli,
% published in the International Journal of Computer Vision, September
% 2014. A copy of the author's pre-print version can be found here:
%
% http://isit.u-clermont1.fr/~ab/Publications/Collins_Bartoli_IJCV14.pdf
%
% This package is free and covered by the BSD licence without any warranty. We hope you find this code useful and please cite our paper in your work:
% (c) Toby Collins 2015
%
%
%@article{
%year={2014},
%issn={0920-5691},
%journal={International Journal of Computer Vision},
%volume={109},
%number={3},
%doi={10.1007/s11263-014-0725-5},
%title={Infinitesimal Plane-Based Pose Estimation},
%url={http://dx.doi.org/10.1007/s11263-014-0725-5},
%publisher={Springer US},
%keywords={Plane; Pose; SfM; PnP; Homography},
%author={Collins, Toby and Bartoli, Adrien},
%pages={252-286},
%language={English}
%}
%
%
% This is free software covered by the FreeBSD License (see IPPE_license.txt) with Copyright (c) 2015 Toby Collins

%basic argument checking
if nargin~=5
   error('detectAndMatchFeatures has 5 input arguments.');
end
if nargout>2
   error('detectAndMatchFeatures has 2 output arguments.');
end

assert(nargin ==5);
assert(size(image1,3)==1|size(image1,3)==3);
assert(size(image2,3)==1|size(image2,3)==3);
assert(isa(image1,'uint8'));
assert(isa(image2,'uint8'));

%check whether regions-of-interest are used:
if isempty(image1ROI)
    image1ROI = true(size(image1,1),size(image1,2));
end
if isempty(image2ROI)
    image2ROI = true(size(image2,1),size(image2,2));
end

assert(isa(image1ROI,'logical'));
assert(isa(image1ROI,'logical'));

%convert image to grayscale:
if size(image1,3)==3
    image1 = rgb2gray(image1);
end

if size(image2,3)==3
    image2 = rgb2gray(image2);
end

%perform detection and matching:
switch featureOpts.featureMethod
    case 'SURF'
        %detection:
        pointsTemplate = detectSURFFeatures(image1,'MetricThreshold',200);
        featuresTemplate = extractFeatures(image1,pointsTemplate);
        p = pointsTemplate.Location';

        pointsInput = detectSURFFeatures(image2,'MetricThreshold',200);
        featuresInput = extractFeatures(image2,pointsInput);

        %matching using Lowe's ratio test for rejecting bad matches (see his SIFT paper for details)
        [IDX, D]= knnsearch(featuresInput,featuresTemplate,'K',2);
        vlds = D(:,2)./D(:,1)> featureOpts.loweRatioThreshold;
        q = pointsInput.Location(IDX(:,1),:)';
        q = q(:,vlds);
        p = p(:,vlds);
    case 'ASIFT'
        [p,q] = asiftWrapper(featureOpts.asiftPath,image1,image2);
    otherwise
        error('unknown feature detection method is specified');
end

%keep only features located in the rois:
vlds = interp2(double(image1ROI),p(1,:),p(2,:))==1;
vlds = vlds & interp2(double(image2ROI),q(1,:),q(2,:))==1;
p = p(:,vlds);
q = q(:,vlds);


function [ts] = timestamp_neuralynx(tsl, tsh)

% TIMESTAMP_NEURALYNX merge the low and high part of Neuralynx timestamps
% into a single uint64 value

% Copyright (C) 2007, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% 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: timestamp_neuralynx.m 2885 2011-02-16 09:41:58Z roboos $

if ~isa(tsl, 'uint32') && ~isa(tsl, 'int32')
  error('invalid input');
elseif ~isa(tsh, 'uint32') && ~isa(tsl, 'int32')
  error('invalid input');
end

% convert the 32 bit low and 32 bit high timestamp into a 64 bit integer
dum = zeros(2, length(tsh), 'uint32');
if littleendian
  dum(1,:) = tsl;
  dum(2,:) = tsh;
else
  dum(1,:) = tsh;
  dum(2,:) = tsl;
end

ts = typecast(dum(:), 'uint64');


% totalNum = 1867290;
%
% loopCompress(0.1, '/home/yh/mapModel/2018/08.03/weightVector.txt', '/home/yh/mapModel/2018/08.03/visMatrix/', 50, totalNum, 2040, '/home/yh/mapModel/2018/08.03/gurobi_compress_0.2/');
%
% ratio = salientNumCnt( '/home/yh/mapModel/2018/08.03/gurobi_compress_0.2/', totalNum ) / totalNum


function [sig,delay_offset] = delayline(sig,dt,weight,conf)
%DELAYLINE (fractional) delay line with weights
%
%   Usage: [sig,delay_offset] = delayline(sig,dt,weight,conf)
%
%   Input parameter:
%       sig     - input signal (vector), can be in the form of [N C], or
%                 [M C N], where
%                     N ... samples
%                     C ... channels (most probably 2)
%                     M ... number of measurements
%                 If the input is [M C N], the length of dt and weight has to be
%                 1 or M*C. In the last case the first M entries in dt are
%                 applied to the first channel and so on.
%       dt      - delay / s
%       weight  - amplitude weighting factor
%       conf    - configuration struct (see SFS_config).
%
%   Output parameter:
%       sig             - delayed signal
%       delay_offset    - additional delay / s
%                         This is added by the fractional delayline filters to
%                         all channels. For integer delays this is 0.
%
%   DELAYLINE(sig,dt,weight,conf) implementes a delayline, that delays the given
%   signal by dt samples and applies an amplitude weighting factor. The delay is
%   implemented as integer delay or fractional delay filter, see delayline
%   section in SFS_config for possible settings. As default setting an integer
%   delayline is used.
%
%   See also: get_ir, driving_function_imp_wfs

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% 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.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Configuration ===================================================
% Check for old configuration
if isfield(conf, 'usefracdelay')
    error(['%s: conf.usefracdelay is deprecated, please use conf.delayline', ...
           ' instead. See SFS_config for details.'],upper(mfilename));
end
fs = conf.fs;
delay = conf.delayline;

%% ===== Preparation =====================================================
% --- Reshape signals ---
% Check if the signal is an impulse response given in SOFA conventions [M C N],
% or in usual [N C] convention, where
% M ... number of measurements
% C ... number of channels
% N ... number of samples
if ndims(sig)==3
    [M,C,samples] = size(sig);
    channels = M * C;
    % Reshape [M C N] => [N C*M], this will be redone at the end of the function
    sig = reshape(sig,[channels,samples])';
    reshaped = true;
else
    % Assume standard format [N C]
    [samples,channels] = size(sig);
    reshaped = false;
end

%% ===== Resampling ======================================================
% The resampling is applied independently from the actual fractional/integer
% delay handling performed in the next step. The resampling is redone at the end
% of the file.
% If resampling is used together with the integer delay filter, this is already
% a usage of fractional delay due to the upsampling.
%
switch delay.resampling
case 'none'
    rfactor = 1.0;
    delay_offset = 0;
case 'matlab'
    rfactor = delay.resamplingfactor;
    delay_offset = 0;
    sig = resample(sig,rfactor,1);
case 'pm'
    % === Parks-McClellan linear phase FIR filter ===
    rfactor = delay.resamplingfactor;
    rfilt = pm_filter(rfactor*delay.resamplingorder, 0.9/rfactor, ...
      1/rfactor);
    delay_offset = delay.resamplingorder*rfactor/2;

    sig = reshape(sig,1,channels*samples);
    sig = [sig; zeros(rfactor-1,channels*samples)];
    sig = reshape(sig,rfactor*samples,channels);

    sig = convolution(rfactor*rfilt, sig);
otherwise
    error('%s: "%s": unknown resampling method',upper(mfilename), ...
        delay.resampling);
end

%% ===== Expansion of signals, delays or weights =========================
% --- Expand channels
if channels==1
    channels = max(length(dt),length(weight));
    sig = repmat(sig,[1 channels]);
end

% --- Expand dt and weight ---
% If only single valued time delay and weight is given, create vectors
if channels>1 && length(dt)==1, dt=repmat(dt,[1 channels]); end
if channels>1 && length(weight)==1, weight=repmat(weight,[1 channels]); end

%% ===== Conversion to integer delay =====================================
dt = dt.*rfactor.*fs;  % resampled delay / samples
samples = rfactor.*samples;  % length of resampled signals
switch delay.filter
case 'integer'
    % === Integer delays ===
    idt = round(dt);  % round to nearest integer delay
    delay_offset = delay_offset + 0;
case 'zoh'
    % === Zero-order hold ===
    idt = ceil(dt);  % round to next larger integer delay
    delay_offset = delay_offset + 0;
case 'lagrange'
    % === Lagrange polynomial interpolator ===
    if iseven(delay.filterorder)
        idt = round(dt);  % round delay for even order
    else
        idt = floor(dt);  % floor delay for odd order
    end
    fdt = dt - idt;  % fractional part of delays
    b = lagrange_filter(delay.filterorder,fdt);
    a = ones(1,channels);
    delay_offset = delay_offset + floor(delay.filterorder/2);
case 'thiran'
    % === Thiran's allpass filter for maximally flat group delay ===
    idt = round(dt);  % integer part of delays
    fdt = dt - idt;  % fractional part of delays
    [b,a] = thiran_filter(delay.filterorder,fdt);
    delay_offset = delay_offset + delay.filterorder;
case 'least_squares'
    % ==== Least squares interpolation filter ===
    idt = floor(dt);  % integer part of delays
    fdt = dt - idt;  % fractional part of delays
    b = zeros(delay.filterorder+1,channels);
    for ii=1:channels
        b(:,ii) = general_least_squares(delay.filterorder+1,fdt(ii),0.90);
    end
    a = ones(1,channels);
    delay_offset = delay_offset + floor(delay.filterorder/2);
case 'farrow'
    % === Farrow-structure ===
    % Based on the assumption, that each coefficient h(n) of the fractional
    % delay filter can be expressed as a polynomial in d (frac. delay), i.e.
    %            __
    %           \  NPol
    % h_d(n) ~=  >      c_m(n) d^m
    %           /__m=0
    %
    % For some Filter design methods, e.g. Lagrange Interpolators, this is
    % perfectly possible. For other, a uniform grid of test delays d_q is
    % used to fit the polynomials to the desired coefficient(n) find a set
    % polynomial which approximates each coefficient of the desired filter.
    % This structure allows to perform the convolution independently from
    % the delay and reuse the results of the filter for different delays.
    %                           __
    %                          \  NPol
    % y(n) = h_d(n) * x(n) ~=   >      ( c_m(n)*x(n) ) d^m
    %                          /__m=0
    %
    % The above representation shows that the convolution of the input
    % signal x can be performed by first convolving c_m and x and
    % incorporating the delay d afterwards.
    %
    % number of parallel filters, i.e. order of polynomial + 1
    % Nfilter = delay.filternumber;
    to_be_implemented(mfilename);
otherwise
    error('%s: \"%s\" is an unknown delayline filter', ...
        upper(mfilename),delay.filter);
end
% Apply filter if needed
if exist('a','var') && exist('b','var')
    for ii=1:channels
        sig(:,ii) = filter(b(:,ii),a(:,ii),sig(:,ii));
    end
end

%% ===== Integer delayline ===============================================
% Handling of too long delay values (returns vector of zeros)
idt(abs(idt)>samples) = samples;
% Handle positive or negative delays
for ii=1:channels
    if idt(ii)>=0
        sig(:,ii) = [zeros(idt(ii),1); weight(ii)*sig(1:end-idt(ii),ii)];
    else
        sig(:,ii) = [weight(ii)*sig(-idt(ii)+1:end,ii); zeros(-idt(ii),1)];
    end
end

%% ===== Postprocessing ==================================================
% --- Downsampling ---
if rfactor~=1
    sig = sig(1:rfactor:samples,:);
    delay_offset = delay_offset ./ rfactor;
end
% --- Undo reshape ---
% [N M*C] => [M C N]
if reshaped
    % C might have changed due to replication of single-channel input
    sig = reshape(sig',M,[],size(sig,1));
end
% --- delay_offset in seconds ---
delay_offset = delay_offset / fs;