function spm_htw_from_fit(varargin)
% When fitting a 1st-level SPM model with multiple basis functions,
% reconstructs fitted response at each voxel beta images and basis functions, and
% estimates height (amplitude), time-to-peak, and response width (duration and half-max)
% For each condition. Saves maps of these across voxels, and contrasts
% across height, time-to-peak, and width maps if contrasts are specified.
% This provides contrast images to take to 2nd-level analyses for group statistics when
% using multiple basis functions at the 1st level.
%
% :Usage:
% ::
%
%     function spm_htw_from_fit(varargin)
%
% :NOTES:
%
% This function loads the SPM.mat file in the current directory and uses
% the basis set specified in the loaded SPM structure.
%
% : Required Inputs:
% 	None - run within an SPM first-level analysis directory
%
% :Optional Inputs:
%
%   **'amplitudes'**:
%       do amplitudes only; no contrasts
%
%   **'contrasts'**:
%       do contrasts only; no amplitudes
%
%   **'noamplitudes'**
%        skip create amping images (combination of betas
%        across basis functions)
%
%   **'nocontrasts'**
%        skip creation of contrast images
%        (running contrast images assumes that amplitude images are already
%        created)
%
%   **'all'**
%        will run both the amplitudes and contrasts sections
%
%          - The second way uses CANlab HTW code to estimate height, time to peak,
%            width, and area under the curve (see Lindquist & Wager 2007 for
%            simulations using a version of this code).
%            It requires SCANlab specific functions, in SCN_Core_Support
%            (unlike the deriv. boost).
%            To turn this OFF, enter 'nohtw' as an optional argument
%
%   **'startend'**
%        followed by starting and ending values in seconds for amplitude
%        estimation window (for HTW estimation only).
%        If you do not enter this, it will show you a plot and ask you to pick
%        these values.
%        If you enter them here as inputs, you can skip the interactive step and
%        loop over subjects.
%
%   **'condition_numbers'**
%        followed by which index numbers in your list should
%        be used to calculate h, t, w from.  You should use this if you
%        are entering regressors of no interest, besides the intercepts.
%
% :Important for Contrasts:
%    disp('Using contrasts entered as F-contrasts. Assuming the first contrast vector in each F-contrast '
%
%    disp('is a contrast of interest across the CANONICAL basis function regressors.')
%
% :Output:
% ::
% Reconstructed amplitude, time-to-peak, and duration (width) images for each event type
% e.g., for Event type (condition) 001:
% htw_amplitude_001.nii
% htw_time_to_peak_001.nii
% htw_width_001.nii
% htw_area_under_curve_001.nii
%
% Contrasts across these, using names stored in SPM.xCon.name:
% e.g.,
% con_htw_ampl_targetvsstandards5.nii
% con_htw_time_targetvsstandards5.nii
% con_htw_widt_targetvsstandards5.nii
% con_htw_area_targetvsstandards5.nii
%
% :Examples:
% ::
%
%    % RUN THIS IN COMMAND WINDOW TO BATCH
%    subj = dir('06*')
%    for i = 1:length(subj), cd(subj(i).name), spm_htw_from_fit, cd('..'); end
%
%    % ANOTHER BATCH EXAMPLE:
%    d = dir('remi*'); d = d(cat(2, d.isdir)); [mydirs{1:length(d)}] = deal(d.name)
%    for i = 1:length(mydirs), cd(mydirs{i}), spm_htw_from_fit('all', 'nodb', 'startend', [4 15]), cd('..'); end
%
%    %An example for an event-related design, specifying condition numbers to get HTW from:
%    spm_htw_from_fit('all', 'contrasts', 'condition_numbers', 1:14, 'startend', [4 10]);
%
%    % CALCULATE CONTRASTS ONLY ON ALREADY-ESTIMATED HTW IMAGES
%    spm_htw_from_fit('contrasts','condition_numbers',1:14);
%
% :References:
% ::
% Lindquist, M. A. & Wager, T. D. (2007). Validity and power in hemodynamic response modeling:
% a comparison study and a new approach. Human Brain Mapping. 8:764-84.
%
% Lindquist, M. A., Meh Loh, J., Atlas, L. Y. & Wager T. D. (2009). Modeling the hemodynamic
% response function in fMRI: efficiency, bias and mis-modeling. Neuroimage. 45:187-98.

% Notes:
% This function is adapted from apply_derivative_boost.m, which was
% originally intended to implement Calhoun derivative boost, but this is
% now deprecated.
% Original documentation:
%        In addition, 'amplitudes' now has two separate parts:
%          - The first uses Vince Calhoun's derivative boost (Calhoun, 2004) to
%             estimate amplitudes.  NOTE: *We have not worked out the scaling yet, so
%             I'm not sure this is working right*
%             To turn this OFF, enter 'nodb' as an optional argument
%

    spmname = fullfile(pwd, 'SPM.mat');     % SETUP INPUTS
    if ~exist(spmname, 'file'), error('You must be in an SPM 1st-level results directory with SPM5 SPM.mat file.'); end

    load(spmname);

    nbf = size(SPM.xBF.bf, 2);

    docontrasts = 1;
    doamps = 1;
    nodb = 1;
    nohtw = 0;
    condition_numbers = [];
    do_downsample = [];          % downsample; default = 1 sec if units are seconds (1/dt)

    for i = 1:length(varargin)
        if ischar(varargin{i})
            switch varargin{i}
                % reserved keywords
                case 'all', docontrasts = 1; doamps = 1;

                case 'amplitudes', doamps = 1; docontrasts = 0;

                case 'contrasts', docontrasts = 1; doamps = 0;

                case 'noamplitudes', doamps = 0;

                case 'nocontrasts', docontrasts = 0;

                case 'db', nodb = 0; % original DB estimation (legacy, deprecated)

                case 'nohtw', nohtw = 1; % skip HTW estimation

                case 'startend', startend = varargin{i + 1};  % starting and ending values in seconds for amplitude estimation window (for HTW estimation only).

                case 'condition_numbers',  condition_numbers = varargin{i + 1};

                case 'nodownsample', do_downsample = 0;

                case 'downsample', do_downsample = varargin{i + 1};

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

    if isempty(do_downsample)
        % default downsampling
        do_downsample = round(1 ./ SPM.xBF.dt);
    end

    if ~(docontrasts || doamps)
        disp('Nothing to do! Enter ''contrasts'' ''amplitudes'' or ''all'' as input argument.');
        return
    end

    % ---------------------------------------------
    % FILE NAMES and imgtype
    % ---------------------------------------------

    imgs = dir(sprintf(['beta*img'])); imgs = char(imgs.name);

    if isempty(imgs)
        imgs = dir(sprintf(['beta*nii'])); imgs = char(imgs.name);
        imgtype = '.nii';
    else
        imgtype = '.img';
    end

    if isempty(imgs)
        error('Cannot find beta*img or beta*nii files in current folder')
    end

    n = size(imgs, 1);

    if doamps
        % ---------------------------------------------
        % ---------------------------------------------

        % ESTIMATE AMPLITUDES

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

        fprintf('Found %3.0f beta images', n); fprintf('\n');

        %load(spmname);
        nsess = length(SPM.Sess);
        fprintf('I think there are %3.0f sessions (runs)', nsess); fprintf('\n');

        if ~isempty(condition_numbers)
            wh_intercept = true(1, size(imgs, 1));  % exclude these
            wh_intercept(condition_numbers) = 0;
            wh_intercept = find(wh_intercept);

            fprintf('\nIncluding only these images: ');
            fprintf('%3.0f ', condition_numbers);
            fprintf('\n');

        else
            wh_intercept = SPM.xX.iB;
            fprintf('\nI think these images are intercepts, and am not using them: ');
            fprintf('%3.0f ', wh_intercept);
            fprintf('\n');
        end

        imgs(wh_intercept, :) = [];

        %not used; only when using image_eval_function
        %mask_img = './mask.img';

        n = size(imgs, 1);

        if n / nbf ~= round(n / nbf), error('Error!  Wrong number of images for the specified number of basis functions.'); end

        if nodb
            % skip Derivative Boost estimation and go straight to HTW
        else
            % DO DB

            switch nbf
                case 2
                    derivative_case = 'timeonly';
                case 3
                    derivative_case = 'timedispersion';

                otherwise
                    warning('Deriv. Boost only works for SPM canonical hrf with time or time + dispersion derivatives.  This SPM.mat doesn''t match those specs.');
            end

            % Not used; only for image eval function
            % boost = @(b) sign(b(1)) .* sqrt(sum(b .^ 2));

            % ---------------------------------------------
            % CALCULATE
            % ---------------------------------------------
            cond_indx = 1;

            for i = 1: nbf : (n - nbf + 1)

                imgs_cond = imgs(i : i+nbf - 1, :);

                disp('Working on :')
                disp(imgs_cond)

                out_name = sprintf(['db_amplitude_%03d',imgtype ], cond_indx);

                % This code uses SCN lab tools to create images
                % ---------------------------------------------
                % %     y = image_eval_function(imgs_cond, boost, 'mask', mask_img, ...
                % %         'outimagelabels', {out_name});
                % ---------------------------------------------

                % This code uses SPM instead
                % ---------------------------------------------
                switch derivative_case
                    case 'timeonly'
                        spm_imcalc(imgs_cond, out_name, 'sign(i1) .* sqrt(i1.^2 + i2.^2)');
                    case 'timedispersion'
                        spm_imcalc(imgs_cond, out_name, 'sign(i1) .* sqrt(i1.^2 + i2.^2 + i3.^2)');
                    otherwise
                        error('Basis set is incompatible with DB estimation!');
                end

                fprintf('Created %s\n', out_name);
                % ---------------------------------------------

                cond_indx = cond_indx + 1;
            end

            % Get and save names
            db_amp_names = [];
            for i = 1:nsess
                sessnames = char(SPM.Sess(i).Fc.name);
                sessnames = [repmat(sprintf('Sess%02d_', i), size(sessnames, 1), 1) sessnames];
                db_amp_names = strvcat(db_amp_names, sessnames);
            end

            save db_amplitude_names db_amp_names
            disp(db_amp_names);
            disp(' ')
            disp('Saved DB amplitude condition names for each image in db_amplitude_names.mat');

            fprintf('\n*-----------------------------*\nApplied DB successfully\n*-----------------------------*\n')

        end

        if nohtw
            % skip this

        else

            % ---------------------------------------------
            % Estimated amplitude from fit: HTW
            % ---------------------------------------------
            % This code uses SCN lab tools to create images

            disp(' ')
            disp('Next: Estimating amplitude, time to peak, width, and area-under-curve images from fitted response using SCN lab code.')
            disp(' ')

            % downsample bf, if requested
            if do_downsample
                mytimeres = SPM.xBF.dt * do_downsample;
                SPM.xBF.bf = downsample(SPM.xBF.bf, do_downsample);

            else
                mytimeres = SPM.xBF.dt;
            end

            if exist('startend', 'var')
                % just check, and use input values
                if length(startend) ~= 2, error('Startend input must have two values, a starting and ending value in seconds for the amp. estimate window'), end

            else
                % Set range in sec
                htw_from_fit(SPM.xBF.bf, ones(size(SPM.xBF.bf, 2), 1), mytimeres, 'plot', 'verbose');

                disp(' ')
                disp('Enter the range in seconds within which to estimate peak amplitude.')
                disp('Example: type [3 12] and press return for a typical event-related setup.');
                disp('More sustained responses, like pain responses, may require a longer window.');
                disp('This estimates the amplitude of the IMPULSE RESPONSE, before convolution with the stimulus function')
                disp('so if you have an epoch design, a typical window of [3 12] sec is still appropriate.');
                disp('Also note: AUC images are calculated as the area under the curve within the window you specify.')
                disp(' ')
                disp('In the future, you can input ''startend'', [3 12] (for example) to skip interactive selection')
                startend = input('Enter your choice in [ ] and press return: ');
            end

            % Test your choice by showing you a plot
            htwfunction = @(b) htw_from_fit(SPM.xBF.bf, b, mytimeres, 'startval', startend(1), 'endval', startend(2), 'plot', 'verbose');
            htwfunction(ones(size(SPM.xBF.bf, 2), 1))
            drawnow

            % Create without plot option for loop through brain.
            htwfunction = @(b) htw_from_fit(SPM.xBF.bf, b, mytimeres, 'startval', startend(1), 'endval', startend(2));

            disp('Check the screen for a plot of your choice of window.')
            disp(' ')

            % ---------------------------------------------
            % CALCULATE
            % ---------------------------------------------
            cond_indx = 1;

            for i = 1: nbf : (n - nbf + 1)

                imgs_cond = imgs(i : i+nbf - 1, :);

                disp('Working on :')
                disp(imgs_cond)

                clear out_name
                out_name{1} = sprintf(['htw_amplitude_%03d',imgtype], cond_indx);
                out_name{2} = sprintf(['htw_time_to_peak_%03d',imgtype], cond_indx);
                out_name{3} = sprintf(['htw_width_%03d',imgtype], cond_indx);
                out_name{4} = sprintf(['htw_area_under_curve_%03d',imgtype], cond_indx);

                % ---------------------------------------------
                [h, t, w, auc] = image_eval_function(imgs_cond, htwfunction, 'mask', fullfile(pwd, ['mask', imgtype]), ...
                    'outimagelabels', out_name);

                h;t;w;auc;  % we need the outputs above to tell it to write 4 images.
                % ---------------------------------------------

                cond_indx = cond_indx + 1;
            end

            % Get and save names
            htw_amp_names = [];
            for i = 1:nsess
                sessnames = char(SPM.Sess(i).Fc.name);
                sessnames = [repmat(sprintf('Sess%02d_', i), size(sessnames, 1), 1) sessnames];
                htw_amp_names = strvcat(htw_amp_names, sessnames);
            end

            if ~exist(fullfile(pwd, 'db_amplitude_names.mat'), 'file')
                save db_amplitude_names htw_amp_names
            else
                save db_amplitude_names -append htw_amp_names
            end
            disp(htw_amp_names);
            disp(' ')
            disp('Saved HTW amplitude condition names for each image in db_amplitude_names.mat');

            fprintf('\n*-----------------------------*\nApplied HTW estimation successfully\n*-----------------------------*\n')

        end

    end  % amplitudes

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

    % CREATE CONTRAST FOR THIS SUBJECT

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

    if docontrasts

        % Load contrast vectors

        % -----------------------------------------
        %load(spmname);
        % nsess = length(SPM.Sess);

        % Define which indices to exclude from contrasts
        if ~isempty(condition_numbers)
            nconvals = length(SPM.xCon(1).c(:, 1));  % test contrast
            wh_intercept = true(1, nconvals);  % exclude these
            wh_intercept(condition_numbers) = 0;
            wh_intercept = find(wh_intercept);

            fprintf('\nIncluding only these images: ');
            fprintf('%3.0f ', condition_numbers);
            fprintf('\n');

        else
            wh_intercept = SPM.xX.iB;
            fprintf('\nI think these images are intercepts, and am not using them: ');
            fprintf('%3.0f ', wh_intercept);
            fprintf('\n');
        end

        if ~isfield(SPM, 'xCon')
            error('Enter F-contrasts first, with the first contrast vector in each F-contrast the contrast across the CANONICAL basis function.');
        else
            disp('Using contrasts entered as F-contrasts. Assuming the first contrast vector in each F-contrast ')
            disp('is a contrast of interest across the CANONICAL basis function regressors.')
        end

        wh_F = strmatch('F', char(SPM.xCon.STAT), 'exact');

        wh_T = strmatch('T', char(SPM.xCon.STAT), 'exact');

        wh_F = [wh_F wh_T];

        if isempty(wh_F) && isempty(wh_T), error('No F-contrasts or T-contrasts entered yet.'); end

        % All sets of contrast images
        % ------------------------------------------
        if ~nodb

            ampimgs_name = sprintf(['db_amplitude_*',imgtype]);

            ampimgs = dir(ampimgs_name); ampimgs = char(ampimgs.name);

            if ~isempty(ampimgs)
                contrast_image_names_dbamp = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype);
            end

        end

        % HTW amplitude
        % ------------------------------------------
        ampimgs_name = sprintf(['htw_amplitude_*',imgtype]);

        ampimgs = dir(ampimgs_name); ampimgs = char(ampimgs.name);

        if ~isempty(ampimgs)
            contrast_image_names_htwamp = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype);
        else
            disp(['Checked for but did not find: ' ampimgs_name]);
        end

        % HTW time
        % ------------------------------------------
        ampimgs_name = sprintf(['htw_time_to_peak_*',imgtype]);

        ampimgs = dir(ampimgs_name); ampimgs = char(ampimgs.name);

        if ~isempty(ampimgs)
            contrast_image_names_htwtime = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype);
        else
            disp(['Checked for but did not find: ' ampimgs_name]);
        end

        % HTW width
        % ------------------------------------------
        ampimgs_name = sprintf(['htw_width_*',imgtype]);

        ampimgs = dir(ampimgs_name); ampimgs = char(ampimgs.name);

        if ~isempty(ampimgs)
            contrast_image_names_htwwid = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype);
        else
            disp(['Checked for but did not find: ' ampimgs_name]);
        end

        % HTW area
        % ------------------------------------------
        ampimgs_name = sprintf(['htw_area_under_curve_*',imgtype]);

        ampimgs = dir(ampimgs_name); ampimgs = char(ampimgs.name);

        if ~isempty(ampimgs)
            contrast_image_names_htwarea = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype);
        else
            disp(['Checked for but did not find: ' ampimgs_name]);
        end

        check_it = whos('contrast_image_names*');

        if isempty(check_it)
            disp('No valid images found to create contrasts on');

        else
            save('db_amplitude_names', '-append', 'contrast_image_names*');
            disp('Saved lists of contrast image names in db_amplitude_names.mat');
        end

    end % end contrasts

    %% INLINE

end  % main function

function contrast_image_names = calc_contrasts(ampimgs_name, ampimgs, wh_F, SPM, wh_intercept, nbf, imgtype)

    n = size(ampimgs, 1);

    fprintf('Found %3.0f images:\n', n);
    disp(ampimgs)

    disp('Reading image data.')
    V = spm_vol(ampimgs);
    vols = spm_read_vols(V);

    % spm_check_registration(ampimgs);
    % colormap jet

    contrast_image_names = [];

    for i = 1:length(wh_F)

        name = SPM.xCon(wh_F(i)).name;
        disp(['Calculating contrast on: ' name])
        original_name = name;

        name = deblank(name);
        wh_bad = (name == ' ' | name == ',' | name == '.' | name == '^' | name == '~' | name == '''' | name == ':' | name == '*' | name == '%' | name == ';' | name == '@' | name == '&');
        name(wh_bad) = [];

        name = ['con_', ampimgs_name(1:8), '_', name, imgtype];

        c = SPM.xCon(wh_F(i)).c(:, 1);
        c(wh_intercept) = [];
        c = c(1 : nbf : end);

        if length(c) ~= n, error('Contrast is wrong length for some reason!  Coding error in this function?  Or wrong number of db_amplitude images.'); end

        fprintf('Contrast values: ')
        fprintf('%01d ', c)
        fprintf('\n')

        % calculate and save

        contrast_image_calc(name, c, vols, V, original_name)

        disp(['Written: ' name]);

        contrast_image_names = strvcat(contrast_image_names, name);

        disp(' ');

    end

    fprintf('\n*-----------------------------*\nContrasts Done successfully!\n*-----------------------------*\n')
    spm_check_registration(contrast_image_names);

end

function contrast_image_calc(Q, myc, vols, V, original_name)

    % FROM:
    % function contrast_image(Q, myc)
    %
    % Tor Wager
    %
    % Creates a contrast image called Q (do not include path)
    % Given a list of img files P (spm format, with path)
    % and a contrast vector myc
    % In the directory of 1st image in P.

    if ~(length(myc) == size(vols,4))
        error('Contrast vector length is not equal to number of image files.')
    end

    myc2 = zeros(size(vols));

    for i = 1:length(myc)
        myc2(:,:,:,i) = myc(i);
    end

    cvol = vols .* myc2;
    cvol = sum(cvol,4);

    % -------------------------
    % write
    % -------------------------
    dd = fileparts(V(1).fname);
    Q = fullfile(dd, Q);
    Vo = V(1);
    Vo.fname = Q;
    Vo.descrip = ['Contrast ' original_name];

    spm_write_vol(Vo,cvol);

end


function render = tbx_cfg_render
% Configuration file for toolbox 'Rendering'
%__________________________________________________________________________
% Copyright (C) 2008-2016 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: tbx_cfg_render.m 6960 2016-12-05 17:05:09Z guillaume $

if ~isdeployed, addpath(fullfile(spm('dir'),'toolbox','SRender')); end

%--------------------------------------------------------------------------
% images Input Images
%--------------------------------------------------------------------------
images         = cfg_files;
images.tag     = 'images';
images.name    = 'Input Images';
images.help    = {
    'These are the images that are used by the calculator.'
    'They are referred to as i1, i2, i3, etc in the order that they are specified.'
    }';
images.filter  = 'image';
images.ufilter = '.*';
images.num     = [1 Inf];

%--------------------------------------------------------------------------
% expression Expression
%--------------------------------------------------------------------------
expression         = cfg_entry;
expression.tag     = 'expression';
expression.name    = 'Expression';
expression.help    = {
    'Example expressions (f):'
    '    * Mean of six images (select six images)'
    '       f = ''(i1+i2+i3+i4+i5+i6)/6'''
    '    * Make a binary mask image at threshold of 100'
    '       f = ''i1>100'''
    '    * Make a mask from one image and apply to another'
    '       f = ''i2.*(i1>100)'''
    '             - here the first image is used to make the mask, which is applied to the second image'
    '    * Sum of n images'
    '       f = ''i1 + i2 + i3 + i4 + i5 + ...'''
    }';
expression.strtype = 's';
expression.num     = [2  Inf];
expression.val     = {'i1'};

%--------------------------------------------------------------------------
% thresh Surface isovalue(s)
%--------------------------------------------------------------------------
thresh         = cfg_entry;
thresh.tag     = 'thresh';
thresh.name    = 'Surface isovalue(s)';
thresh.help    = {'Enter the value at which isosurfaces through the resulting image is to be computed.'};
thresh.strtype = 'e';
thresh.num     = [1  1];
thresh.val     = {0.5};

%--------------------------------------------------------------------------
% surface Surface
%--------------------------------------------------------------------------
surface      = cfg_branch;
surface.tag  = 'surface';
surface.name = 'Surface';
surface.val  = {expression thresh };
surface.help = {'An expression and threshold for each of the surfaces to be generated.'};

%--------------------------------------------------------------------------
% Surfaces Surfaces
%--------------------------------------------------------------------------
Surfaces        = cfg_repeat;
Surfaces.tag    = 'Surfaces';
Surfaces.name   = 'Surfaces';
Surfaces.help   = {'Multiple surfaces can be created from the same image data.'};
Surfaces.values = {surface };
Surfaces.num    = [0 Inf];

%--------------------------------------------------------------------------
% SExtract Surface Extraction
%--------------------------------------------------------------------------
SExtract      = cfg_exbranch;
SExtract.tag  = 'SExtract';
SExtract.name = 'Surface Extraction';
SExtract.val  = {images Surfaces };
SExtract.help = {'User-specified algebraic manipulations are performed on a set of images, with the result being used to generate a surface file. The user is prompted to supply images to work on and a number of expressions to evaluate, along with some thresholds. The expression should be a standard matlab expression, within which the images should be referred to as i1, i2, i3,... etc. An isosurface file is created from the results at the user-specified threshold.'};
SExtract.prog = @spm_sextract;
SExtract.vout = @vout_sextract;

%--------------------------------------------------------------------------
% SurfaceFile Surface File
%--------------------------------------------------------------------------
SurfaceFile         = cfg_files;
SurfaceFile.tag     = 'SurfaceFile';
SurfaceFile.name    = 'Surface File';
SurfaceFile.help    = {
    'Filename of the surf_*.gii file containing the rendering information.'
    'This can be generated via the surface extraction routine in SPM.'
    'Normally, a surface is extracted from grey and white matter tissue class images, but it is also possible to threshold e.g. an spmT image so that activations can be displayed.'
    };
SurfaceFile.filter  = 'mesh';
SurfaceFile.ufilter = '.*';
SurfaceFile.num     = [1 1];

%--------------------------------------------------------------------------
% Red Red
%--------------------------------------------------------------------------
Red        = cfg_menu;
Red.tag    = 'Red';
Red.name   = 'Red';
Red.help   = {'The intensity of the red colouring (0 to 1).'};
Red.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'};
Red.values = {0 0.2 0.4 0.6 0.8 1};
Red.val    = {1};

%--------------------------------------------------------------------------
% Green Green
%--------------------------------------------------------------------------
Green        = cfg_menu;
Green.tag    = 'Green';
Green.name   = 'Green';
Green.help   = {'The intensity of the green colouring (0 to 1).'};
Green.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
Green.values = {0 0.2 0.4 0.6 0.8 1};
Green.val    = {1};

%--------------------------------------------------------------------------
% Blue Blue
%--------------------------------------------------------------------------
Blue        = cfg_menu;
Blue.tag    = 'Blue';
Blue.name   = 'Blue';
Blue.help   = {'The intensity of the blue colouring (0 to 1).'};
Blue.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
Blue.values = {0 0.2 0.4 0.6 0.8 1};
Blue.val    = {1};

%--------------------------------------------------------------------------
% Color Color
%--------------------------------------------------------------------------
Color      = cfg_branch;
Color.tag  = 'Color';
Color.name = 'Color';
Color.val  = {Red Green Blue};
Color.help = {
    'Specify the colour using a mixture of red, green and blue.'
    'For example, white is specified by 1,1,1, black is by 0,0,0 and purple by 1,0,1.'
    }';

%--------------------------------------------------------------------------
% DiffuseStrength Diffuse Strength
%--------------------------------------------------------------------------
DiffuseStrength        = cfg_menu;
DiffuseStrength.tag    = 'DiffuseStrength';
DiffuseStrength.name   = 'Diffuse Strength';
DiffuseStrength.help   = {'The strength with which the object diffusely reflects light. Mat surfaces reflect light diffusely, whereas shiny surfaces reflect speculatively.'};
DiffuseStrength.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
DiffuseStrength.values = {0 0.2 0.4 0.6 0.8 1};
DiffuseStrength.val    = {0.8};

%--------------------------------------------------------------------------
% AmbientStrength Ambient Strength
%--------------------------------------------------------------------------
AmbientStrength        = cfg_menu;
AmbientStrength.tag    = 'AmbientStrength';
AmbientStrength.name   = 'Ambient Strength';
AmbientStrength.help   = {'The strength with which the object reflects ambient (non-directional) lighting.'};
AmbientStrength.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
AmbientStrength.values = {0 0.2 0.4 0.6 0.8 1};
AmbientStrength.val    = {0.2};

%--------------------------------------------------------------------------
% SpecularStrength Specular Strength
%--------------------------------------------------------------------------
SpecularStrength        = cfg_menu;
SpecularStrength.tag    = 'SpecularStrength';
SpecularStrength.name   = 'Specular Strength';
SpecularStrength.help   = {'The strength with which the object specularly reflects light (i.e. how shiny it is). Mat surfaces reflect light diffusely, whereas shiny surfaces reflect speculatively.'};
SpecularStrength.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
SpecularStrength.values = {0 0.2 0.4 0.6 0.8 1};
SpecularStrength.val    = {0.2};

%--------------------------------------------------------------------------
% SpecularExponent Specular Exponent
%--------------------------------------------------------------------------
SpecularExponent        = cfg_menu;
SpecularExponent.tag    = 'SpecularExponent';
SpecularExponent.name   = 'Specular Exponent';
SpecularExponent.help   = {'A parameter describing the specular reflectance behaviour. It relates to the size of the high-lights.'};
SpecularExponent.labels = {'0.01' '0.1' '10' '100'};
SpecularExponent.values = {0.01  0.1 10 100};
SpecularExponent.val    = {10};

%--------------------------------------------------------------------------
% SpecularColorReflectance Specular Color Reflectance
%--------------------------------------------------------------------------
SpecularColorReflectance        = cfg_menu;
SpecularColorReflectance.tag    = 'SpecularColorReflectance';
SpecularColorReflectance.name   = 'Specular Color Reflectance';
SpecularColorReflectance.help   = {'Another parameter describing the specular reflectance behaviour.'};
SpecularColorReflectance.labels = {'0.0' '0.2' '0.4' '0.6' '0.8'  '1.0'};
SpecularColorReflectance.values = {0 0.2 0.4 0.6  0.8 1};
SpecularColorReflectance.val    = {0.8};

%--------------------------------------------------------------------------
% FaceAlpha Face Alpha
%--------------------------------------------------------------------------
FaceAlpha        = cfg_menu;
FaceAlpha.tag    = 'FaceAlpha';
FaceAlpha.name   = 'Face Alpha';
FaceAlpha.help   = {
    'The opaqueness of the surface.'
    'A value of 1 means it is opaque, whereas a value of 0 means it is transparent.'
    }';
FaceAlpha.labels = {'0.0' '0.2' '0.4' '0.6' '0.8' '1.0'}';
FaceAlpha.values = {0 0.2 0.4 0.6 0.8 1};
FaceAlpha.val    = {1};

%--------------------------------------------------------------------------
% Object Object
%--------------------------------------------------------------------------
Object      = cfg_branch;
Object.tag  = 'Object';
Object.name = 'Object';
Object.val  = {SurfaceFile Color DiffuseStrength AmbientStrength SpecularStrength SpecularExponent SpecularColorReflectance FaceAlpha };
Object.help = {'Each object is a surface (from a surf_*.gii file), which may have a number of light-reflecting qualities, such as colour and shinyness.'};

%--------------------------------------------------------------------------
% Objects Objects
%--------------------------------------------------------------------------
Objects        = cfg_repeat;
Objects.tag    = 'Objects';
Objects.name   = 'Objects';
Objects.help   = {'Several surface objects can be displayed together in different colours and with different reflective properties.'};
Objects.values = {Object };
Objects.num    = [0 Inf];

%--------------------------------------------------------------------------
% Position Position
%--------------------------------------------------------------------------
Position         = cfg_entry;
Position.tag     = 'Position';
Position.name    = 'Position';
Position.help    = {'The position of the light in 3D.'};
Position.strtype = 'e';
Position.num     = [1  3];
Position.val     = {[100 100 100]};

%--------------------------------------------------------------------------
% Light Light
%--------------------------------------------------------------------------
Light      = cfg_branch;
Light.tag  = 'Light';
Light.name = 'Light';
Light.val  = {Position Color};
Light.help = {'Specification of a light source in terms of position and colour.'};

%--------------------------------------------------------------------------
% Lights Lights
%--------------------------------------------------------------------------
Lights        = cfg_repeat;
Lights.tag    = 'Lights';
Lights.name   = 'Lights';
Lights.help   = {'There should be at least one light specified so that the objects can be clearly seen.'};
Lights.values = {Light};
Lights.val    = {Light};
Lights.num    = [0 Inf];

%--------------------------------------------------------------------------
% SRender Surface Rendering
%--------------------------------------------------------------------------
SRender      = cfg_exbranch;
SRender.tag  = 'SRender';
SRender.name = 'Surface Rendering';
SRender.val  = {Objects Lights};
SRender.help = {'This utility is for visualising surfaces.  Surfaces first need to be extracted and saved in surf_*.gii files using the surface extraction routine.'};
SRender.prog = @spm_srender;

%--------------------------------------------------------------------------
% render Rendering
%--------------------------------------------------------------------------
render        = cfg_choice;
render.tag    = 'render';
render.name   = 'Rendering';
render.help   = {'This is a toolbox that provides a limited range of surface rendering options. The idea is to first extract surfaces from image data, which are saved in rend_*.mat files. These can then be loaded and displayed as surfaces. Note that OpenGL rendering is used, which can be problematic on some computers. The tools are limited - and they do what they do.'};
render.values = {SExtract SRender};

%==========================================================================
function dep = vout_sextract(job)
dep = cfg_dep;
for k=1:numel(job.surface),
    dep(k)            = cfg_dep;
    dep(k).sname      = ['Surface File ' num2str(k)];
    dep(k).src_output = substruct('.','SurfaceFile','()',{k});
    dep(k).tgt_spec   = cfg_findspec({{'filter','mesh'}});
end


function out = Pauli(data, sicd_meta)
%PAULI Pauli Decomposition
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if (nargin>1) && isfield(sicd_meta,'ImageFormation') && ...
        isfield(sicd_meta.ImageFormation,'TxRcvPolarizationProc')
    % Try standard linear polarizations first
    HH_ind=find(strcmpi('H:H',sicd_meta.ImageFormation.TxRcvPolarizationProc));
    HV_ind=find(strcmpi('H:V',sicd_meta.ImageFormation.TxRcvPolarizationProc));
    VH_ind=find(strcmpi('V:H',sicd_meta.ImageFormation.TxRcvPolarizationProc));
    VV_ind=find(strcmpi('V:V',sicd_meta.ImageFormation.TxRcvPolarizationProc));
    % Less common circular polarization next in not linear
    if isempty([HH_ind HV_ind VH_ind VV_ind])
        pols = cellfun(@(x) split(x,':'), sicd_meta.ImageFormation.TxRcvPolarizationProc, 'UniformOutput', false);
        HH_ind = find(cellfun(@(x) strcmpi(x{1},'LHC') && ...
            (strcmpi(x{2},'LHC') || x{2}=='H'), pols));
        HV_ind = find(cellfun(@(x) strcmpi(x{1},'LHC') && ...
            (strcmpi(x{2},'RHC') || x{2}=='V'), pols));
        VH_ind = find(cellfun(@(x) strcmpi(x{1},'RHC') && ...
            (strcmpi(x{2},'LHC') || x{2}=='V'), pols));
        VV_ind = find(cellfun(@(x) strcmpi(x{1},'RHC') && ...
            (strcmpi(x{2},'RHC') || x{2}=='H'), pols));
    end
else % Make band assumptions based on order
    switch size(data,3)
        case 2 % Co/cross
            HH_ind = 1; HV_ind = 2; VH_ind = []; VV_ind = [];
        case 3 % HH/cross/VV
            HH_ind = 1; HV_ind = 2; VH_ind = []; VV_ind = 3;
        case 4 % Full
            HH_ind = 1; HV_ind = 2; VH_ind = 3; VV_ind = 4;
    end
end

out = zeros(size(data,1),size(data,2),3);
% Co-pol,cross-pol
if xor(isscalar(HH_ind), isscalar(VV_ind)) && xor(isscalar(HV_ind), isscalar(VH_ind))
    %incoherent assignment (co is magenta, cross is green)
    out(:,:,1) = data(:,:,[HH_ind VV_ind]);
    out(:,:,2) = data(:,:,[HV_ind VH_ind]);
    out(:,:,3) = data(:,:,[HH_ind VV_ind]);
% HH/VV
elseif isscalar(HH_ind) && isscalar(VV_ind) && isempty(HV_ind) && isempty(VH_ind)
    out(:,:,1) = data(:,:,HH_ind) - data(:,:,VV_ind);
    out(:,:,2) = 0;
    out(:,:,3) = data(:,:,HH_ind) + data(:,:,VV_ind);
% HH,cross-pol,VV
elseif isscalar(HH_ind) && isscalar(VV_ind) && xor(isscalar(HV_ind), isscalar(VH_ind))
    out(:,:,1) = data(:,:,HH_ind) - data(:,:,VV_ind);
    out(:,:,2) = 2*data(:,:,[HV_ind VH_ind]);
    out(:,:,3) = data(:,:,HH_ind) + data(:,:,VV_ind);
% Full-pol (HH,HV,VH,VV)
elseif isscalar(HH_ind) && isscalar(VV_ind) && isscalar(HV_ind) && isscalar(VH_ind)
    out(:,:,1) = data(:,:,HH_ind) - data(:,:,VV_ind);
    out(:,:,2) = data(:,:,HV_ind) + data(:,:,VH_ind);
    out(:,:,3) = data(:,:,HH_ind) + data(:,:,VV_ind);
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [p, stats] = symamd2 (S, knobs)
%SYMAMD Symmetric approximate minimum degree permutation.
%    P = SYMAMD2(S) for a symmetric positive definite matrix S, returns the
%    permutation vector p such that S(p,p) tends to have a sparser Cholesky
%    factor than S.  Sometimes SYMAMD works well for symmetric indefinite
%    matrices too.  The matrix S is assumed to be symmetric; only the
%    strictly lower triangular part is referenced.   S must be square.
%    Note that p = amd(S) is much faster and generates comparable orderings.
%    The ordering is followed by an elimination tree post-ordering.
%
%    Note that this function is source code for the built-in MATLAB symamd
%    function.  It has been renamed here to symamd2 to avoid a filename clash.
%    symamd and symamd2 are identical.
%
%    See also SYMAMD, AMD, COLAMD, COLAMD2.
%
%    Example:
%            P = symamd2 (S)
%            [P, stats] = symamd2 (S, knobs)
%
%    knobs is an optional one- to two-element input vector.  If S is n-by-n,
%    then rows and columns with more than max(16,knobs(1)*sqrt(n)) entries are
%    removed prior to ordering, and ordered last in the output permutation P.
%    No rows/columns are removed if knobs(1)<0.  If knobs(2) is nonzero, stats
%    and knobs are printed.  The default is knobs = [10 0].  Note that knobs
%    differs from earlier versions of symamd.

%    Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore
%    Developed in collaboration with J. Gilbert and E. Ng.
%    Acknowledgements: This work was supported by the National Science
%       Foundation, under grants DMS-9504974 and DMS-9803599.

%-------------------------------------------------------------------------------
% perform the symamd ordering:
%-------------------------------------------------------------------------------

if (nargout <= 1 & nargin == 1)						    %#ok
    p = symamd2mex (S) ;
elseif (nargout <= 1 & nargin == 2)					    %#ok
    p = symamd2mex (S, knobs) ;
elseif (nargout == 2 & nargin == 1)					    %#ok
    [p, stats] = symamd2mex (S) ;
elseif (nargout == 2 & nargin == 2)					    %#ok
    [p, stats] = symamd2mex (S, knobs) ;
else
    error('symamd:  incorrect number of input and/or output arguments.') ;
end

%-------------------------------------------------------------------------------
% symmetric elimination tree post-ordering:
%-------------------------------------------------------------------------------

[ignore, q] = etree (S (p,p)) ;
p = p (q) ;

%    stats is an optional 20-element output vector that provides data about the
%    ordering and the validity of the input matrix S.  Ordering statistics are
%    in stats (1:3).  stats (1) = stats (2) is the number of dense or empty
%    rows and columns ignored by SYMAMD and stats (3) is the number of
%    garbage collections performed on the internal data structure used by
%    SYMAMD (roughly of size 8.4*nnz(tril(S,-1)) + 9*n integers).
%
%    MATLAB built-in functions are intended to generate valid sparse matrices,
%    with no duplicate entries, with ascending row indices of the nonzeros
%    in each column, with a non-negative number of entries in each column (!)
%    and so on.  If a matrix is invalid, then SYMAMD may or may not be able
%    to continue.  If there are duplicate entries (a row index appears two or
%    more times in the same column) or if the row indices in a column are out
%    of order, then SYMAMD can correct these errors by ignoring the duplicate
%    entries and sorting each column of its internal copy of the matrix S (the
%    input matrix S is not repaired, however).  If a matrix is invalid in other
%    ways then SYMAMD cannot continue, an error message is printed, and no
%    output arguments (P or stats) are returned.  SYMAMD is thus a simple way
%    to check a sparse matrix to see if it's valid.
%
%    stats (4:7) provide information if SYMAMD was able to continue.  The
%    matrix is OK if stats (4) is zero, or 1 if invalid.  stats (5) is the
%    rightmost column index that is unsorted or contains duplicate entries,
%    or zero if no such column exists.  stats (6) is the last seen duplicate
%    or out-of-order row index in the column index given by stats (5), or zero
%    if no such row index exists.  stats (7) is the number of duplicate or
%    out-of-order row indices.
%
%    stats (8:20) is always zero in the current version of SYMAMD (reserved
%    for future use).


function [nVertices, nFaces, BstMat] = out_tess_tri( BstFile, OutputFile, isOpenMEEG )
% OUT_TESS_TRI: Exports a surface to a BrainVISA ASCII .tri file.
%
% USAGE:  [nVertices, nFaces] = out_tess_tri( BstFile, OutputFile )
%         [nVertices, nFaces] = out_tess_tri( BstFile/TessMat, OutputFile, isOpenMEEG=0 )
%
% INPUT:
%    - BstFile    : full path to Brainstorm file to export
%    - OutputFile : full path to output file (with '.tri' extension)
%    - isOpenMEEG : if flag set to 1, write the positions in meters instead of millimeters

% @=============================================================================
% 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, 2011-2019

if (nargin < 3) || isempty(isOpenMEEG)
    isOpenMEEG = 0;
end

% ===== LOAD BRAINSTORM SURFACE =====
if ischar(BstFile)
    BstMat = in_tess_bst(BstFile);
else
    BstMat = BstFile;
end

% ===== PREPARE VALUES ======
% Vertices (=> in millimeters)
if ~isOpenMEEG
    BstMat.Vertices = BstMat.Vertices * 1000;
end
% Faces : remove 1 (convert to 0-based indices)
Faces = BstMat.Faces - 1;
% Normals
VertNormals = BstMat.VertNormals;
% Return surface sizes
nVertices = length(BstMat.Vertices);
nFaces = length(BstMat.Faces);

% ===== SAVE FILE =====
% Open file
[fid, message] = fopen(OutputFile, 'w');
if (fid < 0)
    error(['Could not create file : ' message]);
end
% Write vertices and normals
fprintf(fid, '- %g\n', size(BstMat.Vertices,1));
fprintf(fid, '%g %g %g %g %g %g\n', [BstMat.Vertices, VertNormals]');
% Write faces
nfaces = size(Faces,1);
fprintf(fid, '- %g %g %g\n', [nfaces nfaces nfaces]);
fprintf(fid, '%g %g %g\n', Faces');
% Close file
fclose(fid);


% check whether the sequences are of the same length.
% The sequences are aligned from begining. If some sequence is shorter, it
% will be padded with a big negative number, i.e. -1e10, in the first dimension.
%
function [mask, variableLength] = CheckTrajectoryLength(data)

mask = permute(data(1,:,:), [2 3 1]) == -1e10;

variableLength = sum(mask(:));

end


function [objset,trainList]=readAnnotation(Name_batch,theConf)
MaxObjNum=1000000;

minArea=theConf.data.minArea;

fileID=fopen([theConf.data.catedir,'image_class_labels.txt'],'r');
idClassPair=textscan(fileID,'%d %d');
fclose(fileID);
if((length(Name_batch)==1)&&(Name_batch(1)-'0'>0)&&(Name_batch(1)-'0'<=9))
    logInd=idClassPair{2}==str2num(Name_batch);
    imgIds=idClassPair{1}(logInd);
else
    imgIds=idClassPair{1};
end

fileID=fopen([theConf.data.catedir,'train_test_split.txt'],'r');
idTrainTestPair=textscan(fileID,'%d %d');
fclose(fileID);
trainList=find(idTrainTestPair{2}==1);

fileID=fopen([theConf.data.catedir,'classes.txt'],'r');
batchClassnamePair=textscan(fileID,'%d %s');
fclose(fileID);
%logInd=batchClassnamePair{1}==str2num(Name_batch);
%classname=batchClassnamePair{2}(logInd);

fileID=fopen([theConf.data.catedir,'images.txt'],'r');
idNamePair=textscan(fileID,'%d %s');
fclose(fileID);
[~,idx,~]=intersect(idNamePair{1},imgIds);
imgnames=idNamePair{2}(idx);

fileID=fopen([theConf.data.catedir,'bounding_boxes.txt'],'r');
idBndboxPair=textscan(fileID,'%d %d %d %d %d');
fclose(fileID);
[~,idx,~]=intersect(idBndboxPair{1},imgIds);
x=idBndboxPair{2}(idx);
y=idBndboxPair{3}(idx);
width=idBndboxPair{4}(idx);
height=idBndboxPair{5}(idx);

clear logInd fileID idClassPair idNamePair idBndboxPair

objset(MaxObjNum).folder=[];
objset(MaxObjNum).filename=[];
objset(MaxObjNum).name=[];
objset(MaxObjNum).bndbox=[];
objset(MaxObjNum).ID=[];

j=0;
for i=1:length(imgnames)
    filename=imgnames(i);
    words=strsplit(filename{1},'.');
    classID=str2num(words{1});
    logInd=batchClassnamePair{1}==classID;
    classname=batchClassnamePair{2}(logInd);

    name=classname;
    bndbox.xmin=int2str(x(i));
    bndbox.xmax=int2str(x(i)+width(i));
    bndbox.ymin=int2str(y(i));
    bndbox.ymax=int2str(y(i)+height(i));
    if(~IsAreaValid(bndbox,minArea))
        continue;
    end
    j=j+1;
    objset(j).folder='.';
    objset(j).filename=filename{1};
    objset(j).name=name{1};
    objset(j).bndbox=bndbox;
    objset(j).ID=j;
    if(j>MaxObjNum)
        error('MaxObjNum is too small.');
    end
end
objset=objset(1:j);

%load([theConf.output.dir,Name_batch,'/sampling.mat'],'validObj');
%objset=objset(validObj);
for i=1:length(objset)
    objset(i).ID=i;
end
end

function pd=IsAreaValid(bndbox,minArea)
xmin=str2double(bndbox.xmin);
xmax=str2double(bndbox.xmax);
ymin=str2double(bndbox.ymin);
ymax=str2double(bndbox.ymax);
if((xmax-xmin+1)*(ymax-ymin+1)>=minArea)
    pd=true;
else
    pd=false;
end
end


function map = yellowred(n)
%CHROMAJS returns a divergent lightyellow-red color map
%
%   Usage: m = yellowred([n])
%
%   Input parameters:
%       n - optional length of colormap (default uses the figure default length)
%
%   Output parameters:
%       m - colormap [n 3]
%
%   YELLOWRED(N) returns an N-by-3 matrix containing a divergent colormap.
%   Without a given N the same length as the current figure's colormap is used.
%   For details on the colormap have a look at: https://bit.ly/2vC3Ogr
%
%   To change the colormap current figure run: colormap(yellowred)
%
% See also: generate_colormap, moreland, plot_sound_field

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

%% ===== Checking input parameters =======================================
nargmin = 0;
nargmax = 1;
narginchk(nargmin,nargmax);
if nargin < nargmax
    n = size(get(gcf,'colormap'),1);
end

%% ===== Computation =====================================================
table = [ 255, 255, 224
255, 223, 184
255, 188, 148
255, 151, 119
255, 105,  98
238,  66,  86
210,  31,  71
176,   6,  44
139,   0,   0];
map = generate_colormap(table,n);


classdef StructuredEdgeDetection < handle
    %STRUCTUREDEDGEDETECTION  Class implementing edge detection algorithm
    %
    % As described in [Dollar2013].
    %
    % ### Structured forests for fast edge detection
    %
    % This module contains implementations of modern structured edge detection
    % algorithms, i.e. algorithms which somehow takes into account pixel
    % affinities in natural images.
    %
    % ![image01](https://docs.opencv.org/3.3.1/01.jpg)
    % ![image02](https://docs.opencv.org/3.3.1/02.jpg)
    % ![image03](https://docs.opencv.org/3.3.1/03.jpg)
    % ![image04](https://docs.opencv.org/3.3.1/04.jpg)
    % ![image05](https://docs.opencv.org/3.3.1/05.jpg)
    % ![image06](https://docs.opencv.org/3.3.1/06.jpg)
    % ![image07](https://docs.opencv.org/3.3.1/07.jpg)
    % ![image08](https://docs.opencv.org/3.3.1/08.jpg)
    % ![image09](https://docs.opencv.org/3.3.1/09.jpg)
    % ![image10](https://docs.opencv.org/3.3.1/10.jpg)
    % ![image11](https://docs.opencv.org/3.3.1/11.jpg)
    % ![image12](https://docs.opencv.org/3.3.1/12.jpg)
    %
    % ## References
    % [Dollar2013]:
    % > Piotr Dollar and C Lawrence Zitnick. "Structured forests for fast edge
    % > detection". In IEEE International Conference on Computer Vision (ICCV)
    % > 2013, pages 1841-1848.
    %
    % [Lim2013]:
    % > Joseph J Lim, C Lawrence Zitnick, and Piotr DollAr. "Sketch tokens: A
    % > learned mid-level representation for contour and object detection".
    % > In IEEE Conference on Computer Vision and Pattern Recognition (CVPR),
    % > 2013, pages 3158-3165.
    %
    % See also: cv.StructuredEdgeDetection.StructuredEdgeDetection
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    methods
        function this = StructuredEdgeDetection(model, varargin)
            %STRUCTUREDEDGEDETECTION  The only constructor
            %
            %     obj = cv.StructuredEdgeDetection(model)
            %     obj = cv.StructuredEdgeDetection(model, howToGetFeatures)
            %
            % ## Input
            % * __model__ name of the file where the model is stored.
            % * __howToGetFeatures__ optional name of MATLAB M-function that
            %   implements custom feature extractor. A helper function for
            %   training part of: "P. Dollar and C. L. Zitnick. Structured
            %   Forests for Fast Edge Detection, 2013". You need it only if
            %   you would like to train your own forest, otherwise leave it
            %   unspecified for the default implementation. See example below.
            %
            % ## Example
            % The following is an example of a custom feature extractor
            % MATLAB function:
            %
            %     % This function extracts feature channels from src. The
            %     % StructureEdgeDetection uses this feature space to detect
            %     % edges.
            %     function features = myRFFeatureGetter(src, opts)
            %         % src: source image to extract features
            %         % features: output n-channel floating-point feature matrix
            %         % opts: struct of options
            %         gnrmRad = opts.normRad;    % gradientNormalizationRadius
            %         gsmthRad = opts.grdSmooth; % gradientSmoothingRadius
            %         shrink = opts.shrink;      % shrinkNumber
            %         outNum = opts.nChns;       % numberOfOutputChannels
            %         gradNum = opts.nOrients;   % numberOfGradientOrientations
            %
            %         nsize = [size(src,1) size(src,2)] ./ shrink;
            %         features = zeros([nsize outNum], 'single');
            %         % ... here your feature extraction code
            %     end
            %
            % TODO: Custom extractor is not internally used in the current
            % cv.StructuredEdgeDetection implementation. See this
            % [tutorial](https://docs.opencv.org/3.3.1/d2/d59/tutorial_ximgproc_training.html)
            % for more information about training your own structured forest
            % (it uses an external MATLAB toolbox for the training part).
            %
            % See also: cv.StructuredEdgeDetection.detectEdges
            %
            this.id = StructuredEdgeDetection_(0, 'new', model, varargin{:});
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.StructuredEdgeDetection
            %
            if isempty(this.id), return; end
            StructuredEdgeDetection_(this.id, 'delete');
        end
    end

    %% Algorithm
    methods
        function clear(this)
            %CLEAR  Clears the algorithm state
            %
            %     obj.clear()
            %
            % See also: cv.StructuredEdgeDetection.empty,
            %  cv.StructuredEdgeDetection.load
            %
            StructuredEdgeDetection_(this.id, 'clear');
        end

        function b = empty(this)
            %EMPTY  Checks if detector object is empty
            %
            %     b = obj.empty()
            %
            % ## Output
            % * __b__ Returns true if the detector object is empty (e.g in the
            %   very beginning or after unsuccessful read).
            %
            % See also: cv.StructuredEdgeDetection.clear,
            %  cv.StructuredEdgeDetection.load
            %
            b = StructuredEdgeDetection_(this.id, 'empty');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm parameters to a file
            %
            %     obj.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in the specified
            % XML or YAML file.
            %
            % See also: cv.StructuredEdgeDetection.load
            %
            StructuredEdgeDetection_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     obj.load(fname)
            %     obj.load(str, 'FromString',true)
            %     obj.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from the specified XML or
            % YAML file (either from disk or serialized string). The previous
            % algorithm state is discarded.
            %
            % See also: cv.StructuredEdgeDetection.save
            %
            StructuredEdgeDetection_(this.id, 'load', fname_or_str, varargin{:});
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = obj.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.StructuredEdgeDetection.save,
            %  cv.StructuredEdgeDetection.load
            %
            name = StructuredEdgeDetection_(this.id, 'getDefaultName');
        end
    end

    %% StructuredEdgeDetection
    methods
        function dst = detectEdges(this, src)
            %DETECTEDGES  The function detects edges in src and draw them to dst
            %
            %     dst = obj.detectEdges(src)
            %
            % ## Input
            % * __src__ source image (RGB, float, in [0;1]) to detect edges.
            %
            % ## Output
            % * __dst__ destination image (grayscale, float, in [0;1]) where
            %   edges are drawn.
            %
            % The algorithm underlies this function is much more robust to
            % texture presence, than common approaches, e.g. cv.Sobel.
            %
            % See also: cv.Sobel, cv.Canny
            %
            dst = StructuredEdgeDetection_(this.id, 'detectEdges', src);
        end

        function orientation_image = computeOrientation(this, edge_image)
            %COMPUTEORIENTATION  Computes orientation map from edge image
            %
            %     orientation_image = obj.computeOrientation(edge_image)
            %
            % ## Input
            % * **edge_image** edge image from `detectEdges` function.
            %
            % ## Output
            % * **orientation_image** orientation image.
            %
            % See also: cv.StructuredEdgeDetection.detectEdges
            %
            orientation_image = StructuredEdgeDetection_(this.id, 'computeOrientation', edge_image);
        end

        function dst = edgesNms(this, edge_image, orientation_image, varargin)
            %EDGESNMS  Suppress edges (nonmaximum suppression)
            %
            %     dst = obj.edgesNms(edge_image, orientation_image)
            %     dst = obj.edgesNms(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * **edge_image** edge image from `detectEdges` function.
            % * **orientation_image** orientation image from `computeOrientation` function.
            %
            % ## Output
            % * __dst__ suppressed image (grayscale, float, in [0;1]).
            %
            % ## Options
            % * __R__ radius for NMS suppression. default 2
            % * __S__ radius for boundary suppression. default 0
            % * __M__ multiplier for conservative suppression. default 1.0
            % * __IsParallel__ enables/disables parallel computing.
            %   default true
            %
            % The function suppresses edges where edge is stronger in
            % orthogonal direction.
            %
            % See also: cv.StructuredEdgeDetection.detectEdges,
            %  cv.StructuredEdgeDetection.computeOrientation
            %
            dst = StructuredEdgeDetection_(this.id, 'edgesNms', edge_image, orientation_image, varargin{:});
        end
    end

    %% Static functions
    methods (Static)
        function features = getFeatures(src, opts)
            %GETFEATURES  Extracts features from image
            %
            %     features = cv.StructuredEdgeDetection.getFeatures(src, opts)
            %
            % ## Input
            % * __src__ source image to extract features (RGB float in [0;1]).
            % * __opts__ a scalar struct of random forest options
            %   (feature params), with the following fields:
            %   * __normRad__ `gradientNormalizationRadius` gradient
            %     normalization radius.
            %   * __grdSmooth__ `gradientSmoothingRadius` radius for smoothing
            %     of gradients (using convolution with triangle filter).
            %   * __shrink__ `shrinkNumber` amount to shrink channels.
            %   * __nChns__ `numberOfOutputChannels` number of edge
            %     orientation bins for output.
            %   * __nOrients__ `numberOfGradientOrientations` number of
            %     orientations per gradient scale.
            %
            % ## Output
            % * __features__ extracted features.
            %
            % Extracted features are appropriate for StructuredEdgeDetection
            % training.
            %
            % See also: cv.StructuredEdgeDetection.StructuredEdgeDetection
            %
            features = StructuredEdgeDetection_(0, 'getFeatures', src, opts);
        end
    end

end


function [contact, fric_coef, geometry] = RightSoleInside(robot)

    param = sys.GetExtraParams();

    r_foot_frame = robot.Joints(getJointIndices(robot, 'r_leg_akx'));
    contact = CoordinateFrame(...
        'Name','RightSole',...
        'Reference',r_foot_frame,...
        'Offset',[0, param.wf/2, param.hf],...
        'R',[0,0,0]... % z-axis is the normal axis, so no rotation required
        );

    fric_coef.mu = param.mu;
    fric_coef.gamma = param.gamma;

    geometry.la = param.wf/2;
    geometry.lb = param.wf/2;
    geometry.La = param.lt;
    geometry.Lb = param.lh;
end


function tc = tc_visualizeGlm(tc, parent);
%
% tc = tc_visualizeGlm(tc, parent);
%
% Provide graphics that visualize the
% setup and result of applying a general
% linear model to a time course. Requires
% that you first run tc_applyGlm on
% the tc time course struct.
%
%
% ras 04/05.
if nargin<1,    tc = get(gcf,'UserData');     end
if nargin<2,    parent = tc.ui.plot;          end
if parent==gcf | parent==get(gcf, 'CurrentAxes')
    % make a uipanel to fit on the target
    parent = uipanel('Parent', parent, ...
        'Units', 'normalized', ...
        'BackgroundColor', get(gcf, 'Color'), ...
        'Position', [0 0 1 1]);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% apply a GLM if needed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~isfield(tc, 'glm')
    tc = tc_applyGlm(tc);
end
X = tc.glm.designMatrix;
Y = tc.wholeTc(:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clean up existing objects in figure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
otherAxes = findobj('Type', 'axes','Parent', parent);
delete(otherAxes);
otherUiControls = findobj('Type', 'uicontrol', 'Parent',parent);
delete(otherUiControls);
axes('Parent', parent); % shift focus to parent uipanel
delete(gca);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Visualize results: diff't options for deconvolved
% data and non-deconvolved (using HRF)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if isequal(tc.glm.type, 'selective averaging')
    tc = tc_plotDeconvolvedTCs(tc, parent);
    return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% if we got here, the GLM is non-deconvolved, using HRF   %
% show hemodynamic response function used for GLM         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
axes('Parent', parent, 'Position', [.05 .6 .15 .2]); % [.13 .58 .21 .34]
plot(tc.glm.hrf,'k','LineWidth',2);
if tc.params.grid==1
    grid on
end
xlabel('Time, frames')
ylabel('Arbitrary Units')
if isnumeric(tc.params.glmHRF)
    opts = {sprintf('Mean trial for conditions \n %s',num2str(tc.params.snrConds)), ...
        'Boynton gamma function', 'SPM difference-of-gammas' ...
        'Dale & Buckner ''97'};
    hrfName = opts{tc.params.glmHRF};
else
    hrfName = tc.params.glmHRF; hrfName(hrfName=='_') = ' ';
end
title({'HRF function used: ' hrfName}, 'FontWeight', 'bold')
axis tight, axis square, set(gca, 'Box', 'off');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show design matrix                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
axes('Parent', parent, 'Position', [.3 .55 .2 .34]); % [.13 .11 .21 .34]
hImg = imagesc(X);
colormap autumn
nConds = sum(tc.trials.condNums>0);
tickPts = [1:nConds];
tickLabels = {'Individual Conditions' 'DC Predictors for each run'};
set(gca,'XTick',tickPts);
xlabel('Predictors (Conditions + DC)')
ylabel('Time, Frames')
title('Design Matrix', 'FontWeight', 'bold')
set(hImg, 'ButtonDownFcn', 'zoom');
colorbar vert

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% bar beta values for selected conditions %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
axes('Parent', parent, 'Position', [.6 .55 .33 .34]);  %[.57 .58 .33 .34]
sel = setdiff( find(tc_selectedConds(tc)), 1);
tmp = tc.trials.condColors(sel);
for i = 1:length(sel), col(i,:) = tmp{i}; end
xstr = tc_condInitials(tc.trials.condNames(sel));
% for i = 1:length(sel), xstr{i} = num2str(tc.trials.condNums(sel(i))); end
mybar(tc.glm.betas(sel-1), tc.glm.sems(sel-1), xstr, [], col);
% xlabel Predictors
% ylabel('% Signal Change')
set(gca, 'Box', 'off');
if tc.params.grid, grid on; else grid off; end
title('Beta Values', 'FontWeight', 'bold');
ylabel('\beta', 'FontWeight', 'bold', 'Rotation', 0, 'FontSize', 14);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show time course + selected predictors %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if tc.params.legend, ysz = .58; else ysz = .72; end
tc.glm.tcAxes = axes('Parent', parent, 'Position', [.1 .1 ysz .3]);

% construct matrix of predictor functions for each condition:
% (these predictors will include the dc components as well, so we
% won't have them as separate traces):
dc = nConds+1:size(tc.glm.betas, 2);  % indices of DC components
for c = 1:nConds
	predictors(:,c) = X(:,[c dc]) * [tc.glm.betas(:,[c dc])'];
end

% plot
t = [1:length(tc.wholeTc)] .* tc.TR;
hold on
hY = plot(t, tc.wholeTc(:), 'k-', 'LineWidth', 2);
hPred = plot(t, predictors(:,sel-1), 'LineStyle', '-', 'LineWidth', 1.5);
hTot = plot(t, X*tc.glm.betas', 'k-', 'LineWidth', 2);
hRes = plot(t, tc.glm.residual(:), 'k--', 'LineWidth', 1.5);
plot(t, zeros(size(t)), 'k:');
set(hPred, 'Visible', 'off');
set(hRes, 'Visible', 'off');
set(gca, 'Box', 'off');
if tc.params.grid, grid on; else grid off; end
setLineColors([{'k'} tc.trials.condColors(sel) {[.2 .4 .8] [.8 0 0]}]);
xlabel('Time, sec')
ylabel('% Signal')
title('Time course + Scaled Predictors', 'FontWeight', 'bold')
zoom

% for longer time courses, may want a UI control
% scrollbar(tc.glm.tcAxes, tc.wholeTc);
scale = 300;  % max seconds to nicely plot TC
scrollbar(gca, scale);

% create toggles for the different traces
if tc.params.legend, xx = .72; else xx = .9; end
cb = ['tmp = {''off'' ''on''}; val = get(gcbo, ''Value'')+1; ' ...
      'set(get(gcbo,''UserData''), ''Visible'', tmp{val}); ' ...
      'clear tmp val '];

h3 = uicontrol('Style', 'checkbox', 'Units', 'normalized', ...
               'Position', [xx .4 .08 .04], 'String', 'Time Course', ...
               'BackgroundColor', 'w', 'UserData', hY, ...
               'Value', 1, 'Callback', cb);
h4 = uicontrol('Style', 'checkbox', 'Units', 'normalized', ...
               'Position', [xx .32 .08 .04], 'String', 'Predictors', ...
               'BackgroundColor', 'w', 'UserData', hPred, ...
               'Value', 0, 'Callback', cb);
h5 = uicontrol('Style', 'checkbox', 'Units', 'normalized', ...
               'Position', [xx .24 .08 .04], 'String', 'Best Fit', ...
               'BackgroundColor', 'w', 'UserData', hTot, ...
               'Value', 1, 'Callback', cb);
h6 = uicontrol('Style', 'checkbox', 'Units', 'normalized', ...
               'Position', [xx .16 .08 .04], 'String', 'Residual', ...
               'BackgroundColor', 'w', 'UserData', hRes, ...
               'Value', 0, 'Callback', cb);

% lastly, show % variance explained
varEx = sprintf('%2.1f%% Variance Explained', tc.glm.varianceExplained * 100);
uicontrol('Style', 'text', 'Units', 'normalized', ...
          'Position', [xx-.1 .08 .18 .04], 'String', varEx, ...
          'BackgroundColor', 'w','FontSize', 15);

return
% /-------------------------------------------------------------------/ %

% /-------------------------------------------------------------------/ %
function tc = tc_plotDeconvolvedTCs(tc, parent)
% plot the deconvolved time courses in the same way as for the
% default plot, combining mean amplitudes on one side and mean time
% courses on the other.
sel = find(tc_selectedConds(tc));
% sel = sel(sel>1); % no baseline estimated
nConds = length(sel);
frameWindow = unique(round(tc.timeWindow./tc.TR));
prestim = -1 * frameWindow(1);
peakFrames = unique(round(tc.peakPeriod./tc.TR));
bslFrames = unique(round(tc.bslPeriod./tc.TR));
peakFrames = find(ismember(frameWindow,peakFrames));
bslFrames = find(ismember(frameWindow,bslFrames));

%%%%%%%%%%%%%%%%%%%%%%%%
% plot mean amplitudes %
%%%%%%%%%%%%%%%%%%%%%%%%
axes('Position', [.1 .2 .35 .6], 'Parent', parent);
% exp7_plotAmps(tc);
hold on

h1 = gca;

lineWidth = 2;
labels = {};

colors = tc.trials.condColors(sel);
X = tc.trials.condNums(sel);
Y = tc.glm.amps(sel-1);
E = tc.glm.amp_sems(sel-1);
mybar(Y, E, tc_condInitials(tc.trials.condNames(sel)), [], colors);
axis tight
set(gca, 'Box', 'off')
if tc.params.grid==1
    grid on
end

% set line width
htmp = findobj('Type','line','Parent',gca);
set(htmp,'LineWidth',lineWidth);

% add labels
set(gca,'XTick',[1:nConds]);
if tc.params.legend==0, set(gca, 'XTickLabel', labels); end
xlabel('Condition', 'FontWeight', 'bold', 'FontAngle', 'italic');
ylabel('Mean Amplitude, % Signal', 'FontWeight', 'bold', ...
    'FontAngle', 'italic');
title('Deconvolved Amplitudes', 'FontWeight', 'bold');

% set axes to frame bars nicely
% axis auto;
AX = axis;
AX(1:2) = [0 nConds+1];
if isfield(tc.params,'axisBounds') & ~isempty(tc.params.axisBounds)
    AX(3:4) = tc.params.axisBounds(3:4);
end
axis(AX);

%%%%%%%%%%%%%%%%%%%%%
% mean time courses %
%%%%%%%%%%%%%%%%%%%%%
h2 = axes('Position', [.5 .2 .4 .6], 'Parent', parent);
hold on

for i = sel-1
    htmp = errorbar(tc.timeWindow, tc.glm.betas(:,i), tc.glm.sems(:,i));
    set(htmp, 'Color', tc.trials.condColors{i+1}, 'LineWidth', 2);
end

% indicate the peak and baseline periods, if selected
if tc.params.showPkBsl==1
    AX = axis;
    plot(tc.bslPeriod, repmat(AX(3),size(tc.bslPeriod)), ...
        'k', 'LineWidth', 3.5);
    plot(tc.peakPeriod, repmat(AX(4),size(tc.peakPeriod)), ...
        'r', 'LineWidth', 3.5);
end

if tc.params.grid==1
    grid on
end

if isfield(tc.params,'axisBounds') & ~isempty(tc.params.axisBounds)
    axis(h2, tc.params.axisBounds);
end

xlabel('Trial time, secs', 'FontWeight', 'bold', 'FontAngle', 'italic');
ylabel('% Signal', 'FontWeight', 'bold', 'FontAngle', 'italic');
title('Deconvolved Time Courses', 'FontWeight', 'bold');

AX = axis;
txt = sprintf('Variance Explained: %3.1f%%', 100*tc.glm.varianceExplained);
text(AX(1) + .1*diff(AX(1:2)), AX(3) + .9*diff(AX(3:4)), txt, ...
           'FontSize', 12, 'FontWeight', 'bold');

return


function [initXOptions, optionsAll] = hsvargplvmInitXOptions(Ytr, options, globalOpt)

stackedOpt = globalOpt.stackedInitOpt;

%--- Here we have the option of using Bayesian GPLVM or GPLVM for
% initialising the latent spaces. If this is the case, train the
% corresponding models
optionsAll = hsvargplvmCreateOptions(Ytr, options, globalOpt);
initXOptions = cell(1, options.H);
for h=1:options.H
    if strcmp(optionsAll.initX{h}, 'vargplvm') | strcmp(optionsAll.initX{h}, 'fgplvm')
        initXOptions{h}{1} = optionsAll;
        % DOn't allow the D >> N trick for layers > 1
        if h~=1
            if isfield(initXOptions{h}{1}, 'enableDgtN')
                initXOptions{h}{1}.enableDgtN = false;
            end
        end
        initXOptions{h}{1}.latentDim = optionsAll.Q{h};
        initXOptions{h}{1}.numActive = optionsAll.K{h}{1};
        initXOptions{h}{1}.kern = optionsAll.kern{h}{1};
        initXOptions{h}{1}.initX = 'ppca';
        initXOptions{h}{1}.initSNR = 90;
        initXOptions{h}{1}.numActive = 50;
        initXOptions{h}{2} = 160;
        initXOptions{h}{3} = 30;
        if ~isempty(stackedOpt)
            if isfield(stackedOpt, 'stackedInitVardistIters') && ~isempty(stackedOpt.stackedInitVardistIters)
                initXOptions{h}{2} = stackedOpt.stackedInitVardistIters;
            end
            if isfield(stackedOpt, 'stackedInitIters') && ~isempty(stackedOpt.stackedInitIters)
                initXOptions{h}{3} = stackedOpt.stackedInitIters;
            end
            if isfield(stackedOpt, 'stackedInitSNR') && ~isempty(stackedOpt.stackedInitSNR)
                initXOptions{h}{1}.initSNR = stackedOpt.stackedInitSNR;
            end
            if isfield(stackedOpt, 'stackedInitK') && ~isempty(stackedOpt.stackedInitK)
                initXOptions{h}{1}.numActive = stackedInitK;
            end
        end
    elseif ~isempty(stackedOpt) && (iscell(stackedOpt) && ~isempty(stackedOpt{h}))
        initXOptions{h} = stackedOpt{h};
    else
        initXOptions{h} = {};
    end
end


function planC = getFFDCND(planC,leak,sigma,kernelSize)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%leak = 0.035; %0.018; %leakage in per *100

%sigma = 0.14; %0.02

%kernelSize = 73; %5

indexS = planC{end};

doseNum = 1;

i = find(strcmpi({planC{indexS.structures}.structureName}, 'skin'));

if isempty(i)
    i = find(strcmpi({planC{indexS.structures}.structureName}, 'Body'));
end

if isempty(i)
    i = find(strcmpi({planC{indexS.structures}.structureName}, 'External'));
end

if isempty(i)
    error('Skin structure must be defined to generate a uniform CT.');
else
    i = i(1);
end

structNum = i;

scanSet = 1;

%currentDir = cd;

% [FileName,path] = uigetfile('*.*','Select DICOM Dose File, RD');
%
% if path == 0
%     errordlg('DICOM Dose File Should exist');
%     error('DICOM Dose File Should exist');
% end
%
% cd(path);
%
% fileList = dir;
%
% filesToRun = {};
%
% matches = strmatch('RD', {fileList.name});

% cd(currentDir);
%
% for i = 1:length(matches)
%     [maxDose, dA] = getDICOMMaxDose([path fileList(matches(i)).name]);
%     dM(i) = maxDose;
% end

for i = 1 : planC{7}.FractionGroupSequence.Item_1.NumberOfBeams

    BeamMeterset = planC{7}.FractionGroupSequence.Item_1.ReferencedBeamSequence.(['Item_' num2str(i)]).BeamMeterset;

    bs = planC{7}.BeamSequence.(['Item_' num2str(i)]);

    LS = getDICOMLeafPositions(bs);

    if ~isfield(LS,'manufacturer') && isfield(planC{7},'Manufacturer')
        LS.manufacturer = planC{7}.Manufacturer;
    elseif ~isfield(LS,'manufacturer') && ~isfield(planC{7},'Manufacturer')
        errordlg('Manufacturer is not specified. Using Varian as default.');
        LS.manufacturer = 'Varian';
    end

    [inflMap, xV, yV, colDividerXCoord, rowDividerYCoord, rowLeafPositions,MLCopening] = getLSInfluenceMapFactor(LS,leak,bs.BeamNumber);

    gA = bs.ControlPointSequence.Item_1.GantryAngle;
    iC = bs.ControlPointSequence.Item_1.IsocenterPosition;

    if ~isfield(planC{7},'PatientSetupSequence')
        disp('Patient Setup Missing. Setting to defalt HSF');
        position = 'HSF';
    else
        if ~isfield(planC{7}.PatientSetupSequence,(['Item_' num2str(i)]))
            position = {planC{7}.PatientSetupSequence.(['Item_' num2str(1)]).PatientPosition};
        else
            position = {planC{7}.PatientSetupSequence.(['Item_' num2str(i)]).PatientPosition};
        end
    end

    isocenter = [];

    if strcmpi(position, 'HFP')
        isocenter.x = iC(1)/10;
        isocenter.y = iC(2)/10;
        isocenter.z = -iC(3)/10;
    else
        isocenter.x = iC(1)/10;
        isocenter.y = -iC(2)/10;
        isocenter.z = -iC(3)/10;
    end

    isodistance = bs.SourceAxisDistance/10;

    isocenter = [isocenter.x,isocenter.y, isocenter.z];

    [DoseF, rDepth, xMin, xMax, yMin, yMax, PBSizeX, PBSizeY, maxFlu] = getDose_UnitD(sigma,kernelSize,bs,inflMap, xV, yV,MLCopening);

    clear inflMap

    [xD, yD, zD] = getDoseXYZVals(planC{indexS.dose}(doseNum));

    [xM, yM, zM] = meshgrid(xD, yD, zD);

    coll3V = scan2Collimator([xM(:) yM(:) zM(:)], (gA/360)*2*pi, 0, 0, isocenter, isodistance);

%    distsquared = sepsq(coll3V', [0 0 0]');

    coll3V = (coll3V./ repmat(coll3V(:,3), [1 3])) * -isodistance;

    toKeep = coll3V(:,1) <= xMax + PBSizeX*(kernelSize+1)/2  & coll3V(:,1) >= xMin - PBSizeX*(kernelSize+1)/2 & coll3V(:,2) >= yMin - PBSizeY*(kernelSize+1)/2  & coll3V(:,2) <= yMax + PBSizeY*(kernelSize+1)/2 ;

    pointsToInterpolate = coll3V(toKeep,:);

    clear coll3V distsquared

    radDepthV = getRadiologicalDepth(xM(toKeep), yM(toKeep), zM(toKeep), (gA/360)*2*pi, isocenter, isodistance, structNum, scanSet, planC);

    dV = finterp3(pointsToInterpolate(:,1), pointsToInterpolate(:,2), radDepthV, DoseF, [(xMin - PBSizeX*(kernelSize+1)/2) PBSizeX (xMax + PBSizeX*(kernelSize+1)/2)], [(yMin - PBSizeY*(kernelSize+1)/2)  PBSizeY (yMax + PBSizeY*(kernelSize+1)/2)], rDepth, 0);

    %Should be checked for now turn off
    %dV = dV./distsquared(toKeep);

    dose3D = zeros(size(xM));

    dose3D(toKeep) = dV;

    %dose3D = dose3D/max(dose3D(:));

    %Normalize to max dose
    %dose3D = dM(i)*(dose3D/max(dose3D(:)));

    %Normalize to dose at isocentr
%     [maxDose, dA] = getDICOMMaxDose([path fileList(matches(i)).name]);
%
%     planC{indexS.dose}(end + 1) = planC{indexS.dose}(1);
%     planC{indexS.dose}(end).doseArray = dA;
%
%     Dose_Cl = getDoseAt(length(planC{indexS.dose}),isocenter(1),isocenter(2),isocenter(3),planC);
%
%     planC{indexS.dose}(end) = [];
%
%     planC{indexS.dose}(end + 1) = planC{indexS.dose}(1);
%     planC{indexS.dose}(end).doseArray = dose3D;
%
%     Dose_N = getDoseAt(length(planC{indexS.dose}),isocenter(1),isocenter(2),isocenter(3),planC);
%
%     planC{indexS.dose}(end) = [];

 %   dose3D = (Dose_Cl/Dose_N)*dose3D;

 dose3D = BeamMeterset*dose3D;

    if ~exist('dose')
        dose = dose3D;
    else
        dose = dose + dose3D;
    end

    clear xM yM zM dose3D;

end

mask = find(planC{8}(1).doseArray);

dose1 = zeros(size(planC{8}(1).doseArray));

dose1(mask) = dose(mask);

planC{8}(end+1) = planC{8}(1);

planC{8}(end).doseArray = dose1;

planC{8}(end).fractionGroupID = 'planCheck_Dose';

%m = max(planC{8}(1).doseArray(:));

mM = mean(planC{8}(1).doseArray(:));

%planC{8}(end).doseArray = m*planC{8}(end).doseArray./max(planC{8}(end).doseArray(:));

planC{8}(end).doseArray = mM*planC{8}(end).doseArray./mean(planC{8}(end).doseArray(:));


function [structVol, planC] = getStructureVol(structNum,planC)
% getStructureVol
% This function returns the abslute volume of a structure. Can be accessed
% from MATLAB command line or from CERR command.
%
% Created DK
% Usage
% structVol = getStructureVol(structNum)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('planC','var')
    global planC
end
indexS = planC{end};
optS    = planC{indexS.CERROptions};

%Get the scan number associated with the requested structure.
scanSet = getStructureAssociatedScan(structNum, planC);

deltaY = planC{indexS.scan}(scanSet).scanInfo(1).grid1Units;

ROIImageSize = [planC{indexS.scan}(scanSet).scanInfo(1).sizeOfDimension1  planC{indexS.scan}(scanSet).scanInfo(1).sizeOfDimension2];

%Get raster segments for structure.
[segmentsM, planC, isError] = getRasterSegments(structNum, planC);
numSegs = size(segmentsM,1);

%Relative sampling of ROI voxels in this place, compared to CT spacing.
%Set when rasterSegments are generated (usually on import).
sampleRate = optS.ROISampleRate;

%Sample the rows
indFullV =  1 : numSegs;
if sampleRate ~= 1
    rV = 1 : length(indFullV);
    rV([rem(rV+sampleRate-1,sampleRate)~=0]) = [];
    indFullV = rV;
end

%Block process to avoid swamping on large structures
if isfield(optS, 'DVHBlockSize') & ~isempty(optS.DVHBlockSize)
    DVHBlockSize = optS.DVHBlockSize;
else
    DVHBlockSize = 5000;
end

blocks = ceil(length(indFullV)/DVHBlockSize);
volsV  = [];
scansV = [];

start = 1;

for b = 1 : blocks

    %Build the interpolation points matrix

    dummy = zeros(1,DVHBlockSize * ROIImageSize(1));
    x1V = dummy;
    y1V = dummy;
    z1V = dummy;
    volsSectionV =  dummy;

    if start+DVHBlockSize > length(indFullV)
        stop = length(indFullV);
    else
        stop = start + DVHBlockSize - 1;
    end

    indV = indFullV(start:stop);

    mark = 1;

    for i = indV
        tmpV = segmentsM(i,1:10);
        delta = tmpV(5) * sampleRate;
        xV = tmpV(3): delta : tmpV(4);
        len = length(xV);
        rangeV = ones(1,len);
        yV = tmpV(2) * rangeV;
        zV = tmpV(1) * rangeV;
        sliceThickness = tmpV(10);
        %v = delta^2 * sliceThickness;
        v = delta * (deltaY*sampleRate) * sliceThickness;
        x1V(mark : mark + len - 1) = xV;
        y1V(mark : mark + len - 1) = yV;
        z1V(mark : mark + len - 1) = zV;
        volsSectionV(mark : mark + len - 1) = v;
        mark = mark + len;
    end

    %cut unused matrix elements
    x1V = x1V(1:mark-1);
    y1V = y1V(1:mark-1);
    z1V = z1V(1:mark-1);
    volsSectionV = volsSectionV(1:mark-1);

    %Get transformation matrices for both scan and structure.
    transMScan    = getTransM('scan', scanSet, planC);
    transMStruct  = getTransM('struct', structNum, planC);

    %Forward transform the structure's coordinates.
    if ~isempty(transMStruct)
        [x1V, y1V, z1V] = applyTransM(transMStruct, x1V, y1V, z1V);
    end

    %Back transform the coordinates into the scans' coordinate system.
    if ~isempty(transMScan)
        [x1V, y1V, z1V] = applyTransM(inv(transMScan), x1V, y1V, z1V);
    end

    %Interpolate.
    [scansSectionV] = getScanAt(scanSet, x1V, y1V, z1V, planC);

    scansV = [scansV, scansSectionV];
    volsV  = [volsV, volsSectionV];

    start = stop + 1;

end

volsV = volsV * sampleRate^2;  %must account for sampling rate!
structVol = sum(sum(volsV));


%%*******************************************************************
%% HSDHKMpred: Compute (dX,dy,dZ) for the H..K..M direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [par,dX,dy,dZ,coeff,L,hRd] = ...
    HSDHKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol)

global schurfun schurfun_par
%%
%% compute HKM scaling
%%
Zinv = cell(size(blk,1),1);   dd = cell(size(blk,1),1);
gamx = cell(size(blk,1),1);   gamz = cell(size(blk,1),1);
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});
    numblk = length(pblk{2});
    if strcmp(pblk{1},'l')
        Zinv{p} = 1./Z{p};
        dd{p} = X{p}./Z{p};
    elseif strcmp(pblk{1},'q')
        gaptmp  = qops(pblk,X{p},Z{p},1);
        gamz2   = qops(pblk,Z{p},Z{p},2);
        gamz{p} = sqrt(gamz2);
        Zinv{p} = qops(pblk,-1./gamz2,Z{p},4);
        dd{p}   = qops(pblk,gaptmp./gamz2,ones(n,1),4);
    elseif strcmp(pblk{1},'s')
        if (numblk == 1)
            Zinv{p} = Prod2(pblk,full(invZchol{p}),invZchol{p}',1);
        else
            Zinv{p} = Prod2(pblk,invZchol{p},invZchol{p}',1);
        end
    end
end
par.Zinv = Zinv; par.gamx = gamx; par.gamz = gamz; par.dd = dd;
%%
%% compute schur matrix
%%
m = par.m;
schur = sparse(m+2,m+2);
UU = [];  EE = [];
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if strcmp(pblk{1},'l')
        [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,par.dd);
    elseif strcmp(pblk{1},'q');
        [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,par.dd,par.Zinv,X);
    elseif strcmp(pblk{1},'s')
        if isempty(schurfun{p})
            schur = schurmat_sblk(blk,At,par,schur,p,X,par.Zinv);
        elseif ischar(schurfun{p})
            if ~isempty(par.permZ{p})
                Zpinv = Zinv{p}(par.permZ{p},par.permZ{p});
                Xp = X{p}(par.permZ{p},par.permZ{p});
            else
                Xp = X{p};
                Zpinv = Zinv{p};
            end
            schurtmp = feval( schurfun{p}, Xp,Zpinv,schurfun_par(p,:));
            schur = schur + schurtmp;
        end
    end
end
%%
%% compute rhs
%%
[rhs,EinvRc,hRd] = HSDHKMrhsfun(blk,At,par,X,Z,rp,Rd,sigmu);
%%
%% solve linear system
%%
par.addschur   = par.kap/par.tau;
schur(m+1,m+1) = schur(m+1,m+1) + par.kap/par.tau;
schur(m+2,m+2) = schur(m+2,m+2) + par.addschur;
[xx,coeff,L] = HSDlinsysolve(par,schur,UU,EE,par.Umat,rhs);
%%
%% compute (dX,dZ)
%%
[par,dX,dy,dZ] = HSDHKMdirfun(blk,At,par,Rd,EinvRc,X,xx);
%%*******************************************************************


%{
 * Copyright (C) 2020-2030, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function [LiDARTag, AprilTag] = get4Corners(opt, mat_file_path, bagfile, target_len, pc_iter, num_scan)
    pc = loadPointCloud(mat_file_path, bagfile);
    X = getPayload(pc, pc_iter, num_scan);

    % cost
    opt = optimizeCost(opt, X, target_len);
    target_lidar = [0 -target_len/2 -target_len/2 1;
                    0 -target_len/2  target_len/2 1;
                    0  target_len/2  target_len/2 1;
                    0  target_len/2 -target_len/2 1]';
    LiDARTag.corners = inv(opt.H_opt) * target_lidar;
    LiDARTag.corners = sortrows(LiDARTag.corners', 3, 'descend')';
    LiDARTag.four_corners_line = point3DToLineForDrawing(LiDARTag.corners);
    LiDARTag.points = X;

%             HCamera = getAprilTagPose(app, app.image_num);
    AprilTag.corners = getAprilTagCorners(bagfile);
%             AprilTag.undistorted_corners = getApriltagUndistortedCorners(app, AprilTag.corners);
    AprilTag.four_corners_line = [];
end


function [ objectIndex ] = GetScenario_waypoint_90Degrees(varargin)
% This scenario is designed to present a waypoint following exercise.

fprintf('[SCENARIO]\tGetting the waypoint following exercise.\n');

% DEFAULT CONFIGURATION
defaultConfig = struct('file','scenario.mat',...
                       'agents',[],...
                       'agentVelocity',0,...
                       'noiseFactor',0,...
                       'plot',0);

% Instanciate the scenario builder
SBinstance = scenarioBuilder();
% PARSE THE USER OVERRIDES USING THE SCENARIO BUILDER
[inputConfig] = SBinstance.configurationParser(defaultConfig,varargin);
% AGENT CONDITIONS
agentIndex = inputConfig.agents;
agentNumber = numel(agentIndex);                    % Declare the number of agents

%% DEFINE THE AGENT CONFIGURATION
% MOVE THROUGH THE AGENTS AND INITIALISE WITH GLOBAL PROPERTIES
fprintf('[SCENARIO]\tAssigning agent definition...\n');
for i=1:agentNumber
    agentIndex{i}.SetGLOBAL('globalPosition',[0;0;0] + inputConfig.noiseFactor*randn(3,1));
    agentIndex{i}.SetGLOBAL('globalVelocity',[inputConfig.agentVelocity;0;0] + inputConfig.noiseFactor*randn(3,1));
    agentIndex{i}.SetGLOBAL('quaternion',[1;0;0;0]);
end

%% DEFINE THE WAYPOINT CONFIGURATION
fprintf('[SCENARIO]\tBuilding the new scenario...\n');
waypointRadius = 0.1;
nameString = sprintf('WP-%s',agentIndex{1}.name);
waypointPositions = [ 20 20;
                       0 20;
                       0 20];

% ASSIGN AGENT GLOBAL PROPERTIES, ONE SIDE OF THE RINGS TO THE OTHER
waypointIndex{1} = waypoint('radius',waypointRadius,'name',nameString);
waypointIndex{1}.SetGLOBAL('globalPosition',waypointPositions(:,1));
waypointIndex{1}.SetGLOBAL('globalVelocity',zeros(3,1));
waypointIndex{1}.SetGLOBAL('quaternion',[1;0;0;0]);                        % Append properties from the sphereical scenario

waypointIndex{2} = waypoint('radius',waypointRadius,'name',nameString);
waypointIndex{2}.SetGLOBAL('globalPosition',waypointPositions(:,2));
waypointIndex{2}.SetGLOBAL('globalVelocity',zeros(3,1));
waypointIndex{2}.SetGLOBAL('quaternion',[1;0;0;0]);                        % Append properties from the sphereical scenario

for index = 1:numel(waypointIndex)
   waypointIndex{index} = waypointIndex{index}.CreateAgentAssociation(agentIndex{1},1/index);  % Create waypoint with association to agent
end

% BUILD THE COLLECTIVE OBJECT INDEX
objectIndex = horzcat(agentIndex,waypointIndex);
% PLOT THE SCENE
if inputConfig.plot
    SBinstance.plotObjectIndex(objectIndex);
end
% CLEAR THE REMAINING VARIABLES
clearvars -except objectIndex
end