function [favar,favar_hd_estimates]=favar_hdestimates(favar,hd_estimates,n,IRFt,endo,strctident,L_g)

%% prelim
% number of identified shocks & create labels for the contributions
if IRFt==1||IRFt==2||IRFt==3
    identified=n; % fully identified
    labels=endo;
elseif IRFt==4 || IRFt==6 %if the model is identified by sign restrictions or sign restrictions (+ IV)
    identified=size(strctident.signreslabels_shocks,1); % count the labels provided in the sign res sheet (+ IV)
    labels=strctident.signreslabels_shocks; % signreslabels
elseif IRFt==5
    identified=1; % one IV shock
    labels{identified,1}=strcat('IV Shock (',strctident.Instrument,')'); % one label for the IV shock
end

% if favar.onestep==1
%
% else
%     L=favar.L(favar.plotX_index,:);
% end

L=L_g;

% if the number of identified shocks is smaller than the number endo
% variables (not fully identified) and we have blocks, (and sum the
% shock contributions) then we have to compute blocks_index_shocks, to
% know to which blocks the shocks are assigned to, this is identified
% analogue to the Factors by checking for 'Blockidentifier'. [favar.bnames{jj,1} '.']&& identified<n
% endo_exfactors=endo(favar.variablestrings_exfactors);
if favar.blocks==1
    for jj=1:favar.nbnames
        for ii=1:size(labels,1)
            %for ll=1:favar.bnumpc{jj,1}
            %blocks_indexlogical{jj,1}(ll,ii)=strcmp(endo{ii,1},favar.factorlabels_blocks{jj,ll});
            blocks_indexlogical_shocks{jj,1}(ii,1)=contains(labels{ii,1},[favar.bnames{jj,1} '.'])==1;%((ll,ii))
            %end
        end
        %blocks_indexlogical{jj,1}=sum(blocks_indexlogical{jj,1},1);
        favar.blocks_index_shocks{jj,1}=find(blocks_indexlogical_shocks{jj,1}==1);
    end
%     nbnames_shocks=size(favar.blocks_index_shocks,1)
end

% favar.HD.sumShockcontributions=0;

    for jj=1:favar.npltX %for selected information variables
        for ii=1:n %for variables
            for ll=1:n%size(hd_estimates,1) %for shock contributions
                favar_hd_estimates{ll,ii,jj}(1,:)=hd_estimates{ll,ii}(2,:)*L(jj,ii);%summed over variables {ll,ii,jj} %%% summed over contributions {ii,ll,jj}
            end
        end

        for ll=n+1:size(hd_estimates,1) %for the rest of the contributions
            for ii=1:n %for variables
                favar_hd_estimates{ll,ii,jj}(1,:)=hd_estimates{ll,ii}(2,:)*L(jj,ii);
            end
        end
    end


report_this_filefun(mfilename('fullpath'));

ti = [];
ret = [];
figure_w_normalized_uicontrolunits(bmapc)
[px,py]  = ginput(1)
hold on
plot(px,py,'w+','era','normal')
drawnow
for ra = 1:0.25:15
    ra
    retpoint
    l = isnan(ret);
    ret(l) = [];

    [i,j] = find(ret == min(min(ret)));
    if isempty(ret) == 1; ret = nan;end

    tru = (teb-t0b)/(10^(bvg(i,8)-(bvg(i,1)-1.3*bvg(i,9))*6));
    trl = (teb-t0b)/(10^(bvg(i,8)-(bvg(i,1)+1.3*bvg(i,9))*6));

    ti = [ti ; min(ret) ra bvg(i,1)  bvg(i,9)  bvg(i,6) bvg(i,8)  -tru+min(ret) trl-min(ret)];
end

figure
rect = [0.20, 0.70, 0.6, 0.25];
axes('position',rect)
%pl = plot(ti(:,2),ti(:,3))
errorbar(ti(:,2),ti(:,3),ti(:,4),ti(:,4))
%set(pl,'LineWidth',2)
hold on
pl = plot(ti(:,2),ti(:,3),'o')
set(pl,'LineWidth',2)
ylabel('b-value')
xl = get(gca,'Xlim');
xl = [0 16];
set(gca,'Xlim',xl);
matdraw

rect = [0.20, 0.40, 0.6, 0.25];
axes('position',rect)
errorbar(ti(:,2),ti(:,6),2*ti(:,4),2*ti(:,4))
%pl = plot(ti(:,2),ti(:,6))
%set(pl,'LineWidth',2)
hold on
pl = plot(ti(:,2),ti(:,6),'o')
set(pl,'LineWidth',2)
ylabel('a-value')
set(gca,'Xlim',xl);

rect = [0.20, 0.10, 0.6, 0.25];
axes('position',rect)
errorbar(ti(:,2),ti(:,1),ti(:,7),ti(:,8))
%pl = plot(ti(:,2),ti(:,1))
set(pl,'LineWidth',2)
set(gca,'Xlim',xl,'Ylim',[0 150]);
hold on
pl = plot(ti(:,2),ti(:,1),'o')
set(pl,'LineWidth',2)
ylabel('Tr [yrs.]')

xlabel('Radius in [km]')


function varargout = process_average( varargin )
% PROCESS_AVERAGE: Average files, by subject, by condition, or all at once.
%
% USAGE:                    OutputFiles = process_average('Run', sProcess, sInputs)
%                            OutputFile = process_average('AverageFiles', sProcess, sInputs, KeepEvents, isScaleDspm, isWeighted, isMatchRows, isZeroBad)
%                        [sMat,isFixed] = process_average('FixWarpedSurfaceFile', sMat, sInput, sStudyDest)
%  [iGroups, GroupComments, GroupNames] = process_average('SortFiles', sInputs, avgtype)

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

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'Average files';
    sProcess.Category    = 'Custom';
    sProcess.SubGroup    = 'Average';
    sProcess.Index       = 301;
    sProcess.Description = 'https://neuroimage.usc.edu/brainstorm/Tutorials/Averaging#Averaging';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.OutputTypes = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.nInputs     = 1;
    sProcess.nMinFiles   = 2;
    sProcess.isSeparator = 1;
    % Definition of the options
    % === AVERAGE TYPE
    sProcess.options.label1.Comment = 'Group files:';
    sProcess.options.label1.Type    = 'label';
    sProcess.options.avgtype.Comment = {'Everything', 'By subject', 'By folder (subject average)', 'By folder (grand average)', 'By trial group (folder average)', 'By trial group (subject average)', 'By trial group (grand average)'};
    sProcess.options.avgtype.Type    = 'radio';
    sProcess.options.avgtype.Value   = 1;
    % === FUNCTION
    sProcess.options.label2.Comment = 'Function:';
    sProcess.options.label2.Type    = 'label';
    sProcess.options.avg_func.Comment = {'Arithmetic average:  mean(x)', ...
                                         'Average absolute values:  mean(abs(x))', ...
                                         'Root mean square (RMS):  sqrt(sum(x.^2)/N)', ...
                                         'Standard deviation:  sqrt(var(x))', ...
                                         'Standard error:  sqrt(var(x)/N)', ...
                                         'Arithmetic average + Standard deviation', ...
                                         'Arithmetic average + Standard error', ...
                                         'Median:  median(x)'};
    sProcess.options.avg_func.Type    = 'radio';
    sProcess.options.avg_func.Value   = 1;
    % === WEIGHTED AVERAGE
    sProcess.options.weighted.Comment    = 'Weighted average:  mean(x) = sum(Leff_i * x(i)) / sum(Leff_i)';
    sProcess.options.weighted.Type       = 'checkbox';
    sProcess.options.weighted.Value      = 0;
    sProcess.options.weightedlabel.Comment    = '      Leff_i = Effective number of averages for file #i';
    sProcess.options.weightedlabel.Type       = 'label';
    % === KEEP EVENTS
    sProcess.options.keepevents.Comment    = 'Keep all the event markers from the individual epochs';
    sProcess.options.keepevents.Type       = 'checkbox';
    sProcess.options.keepevents.Value      = 0;
    sProcess.options.keepevents.InputTypes = {'data', 'matrix'};
    % === SCALE NORMALIZE SOURCE MAPS (DEPRECATED OPTION AFTER INVERSE 2018)
    sProcess.options.scalenormalized.Comment    = 'Adjust normalized source maps for SNR increase.
Example: dSPM(Average) = sqrt(Navg) * Average(dSPM)';
    sProcess.options.scalenormalized.Type       = 'checkbox';
    sProcess.options.scalenormalized.Value      = 0;
    sProcess.options.scalenormalized.InputTypes = {'results'};
    sProcess.options.scalenormalized.Hidden     = 1;
    % === MATCH ROWS WITH NAMES
    sProcess.options.matchrows.Comment    = 'Match signals between files using their names';
    sProcess.options.matchrows.Type       = 'checkbox';
    sProcess.options.matchrows.Value      = 1;
    sProcess.options.matchrows.InputTypes = {'timefreq', 'matrix'};
    % === EXCLUDE ZEROS FROM THE AVERAGE
    sProcess.options.iszerobad.Comment    = 'Exclude the flat signals from the average (zero at all times)';
    sProcess.options.iszerobad.Type       = 'checkbox';
    sProcess.options.iszerobad.Value      = 1;
    sProcess.options.iszerobad.InputTypes = {'timefreq', 'matrix'};
end

%% ===== FORMAT COMMENT =====
function Comment = FormatComment(sProcess)
    % Function
    if isfield(sProcess.options, 'avg_func')
        switch(sProcess.options.avg_func.Value)
            case 1,  Comment = 'Average: ';
            case 2,  Comment = 'Average/abs: ';
            case 3,  Comment = 'RMS: ';
            case 4,  Comment = 'Standard deviation: ';
            case 5,  Comment = 'Standard error: ';
            case 6,  Comment = 'Average+Std: ';
            case 7,  Comment = 'Average+Stderr: ';
            case 8,  Comment = 'Median: ';
        end
    else
        Comment = 'Average: ';
    end
    % Weighted
    if isfield(sProcess.options, 'weighted') && isfield(sProcess.options.weighted, 'Value') && ~isempty(sProcess.options.weighted.Value) && sProcess.options.weighted.Value
        Comment = ['Weighted ' Comment];
    end
    % Average type
    iAvgType = sProcess.options.avgtype.Value;
    Comment = [Comment, sProcess.options.avgtype.Comment{iAvgType}];
end

%% ===== RUN =====
function OutputFiles = Run(sProcess, sInputs) %#ok<DEFNU>
    % Initialize returned list
    OutputFiles = {};
    % Get current progressbar position
    if bst_progress('isVisible')
        curProgress = bst_progress('get');
        bst_progress('text', 'Grouping files...');
    else
        curProgress = [];
    end
    % Weighted
    if isfield(sProcess.options, 'weighted') && isfield(sProcess.options.weighted, 'Value') && ~isempty(sProcess.options.weighted.Value)
        isWeighted = sProcess.options.weighted.Value;
    else
        isWeighted = 0;
    end
    % Keep events
    if isfield(sProcess.options, 'keepevents') && isfield(sProcess.options.keepevents, 'Value') && ~isempty(sProcess.options.keepevents.Value)
        KeepEvents = sProcess.options.keepevents.Value;
    else
        KeepEvents = 0;
    end
    % Scale normalized source maps (DEPRECATED AFTER INVERSE 2018)
    if isfield(sProcess.options, 'scalenormalized') && isfield(sProcess.options.scalenormalized, 'Value') && ~isempty(sProcess.options.scalenormalized.Value)
        isScaleDspm = sProcess.options.scalenormalized.Value;
    else
        isScaleDspm = 0;
    end
    % Match signals between files using their names
    if isfield(sProcess.options, 'matchrows') && isfield(sProcess.options.matchrows, 'Value') && ~isempty(sProcess.options.matchrows.Value)
        isMatchRows = sProcess.options.matchrows.Value;
    else
        isMatchRows = 1;
    end
    % Exclude zero values
    if isfield(sProcess.options, 'iszerobad') && isfield(sProcess.options.iszerobad, 'Value') && ~isempty(sProcess.options.iszerobad.Value)
        isZeroBad = sProcess.options.iszerobad.Value;
    else
        isZeroBad = 1;
    end

    % Check type of input files
    if ~isempty(strfind(sInputs(1).FileName, 'timefreq_pac')) || ~isempty(strfind(sInputs(1).FileName, 'timefreq_dpac'))
        bst_report('Error', sProcess, [], ['Cannot average PAC maps after their computation.' 10 'Please use the process option "Save average PAC across trials" instead.']);
        return;
    end

    % Group files
    [iGroups, GroupComments] = SortFiles(sInputs, sProcess.options.avgtype.Value);
    % Average each group
    for i = 1:length(iGroups)
        % Set progress bar at the same level for each loop
        if ~isempty(curProgress)
            bst_progress('set', curProgress);
        end
        % Do not process if there is only one input
        if (length(iGroups{i}) == 1)
            bst_report('Warning', sProcess, sInputs(iGroups{i}(1)), 'File is alone in its trial/comment group. Not processed.');
            continue;
        end
        % Set the comment of the output file
        if ~isempty(GroupComments)
            sProcess.options.Comment.Value = GroupComments{i};
        end
        % Compute group average
        OutputFiles{end+1} = AverageFiles(sProcess, sInputs(iGroups{i}), KeepEvents, isScaleDspm, isWeighted, isMatchRows, isZeroBad);
    end
end

%% ===== SORT FILES =====
function [iGroups, GroupComments, GroupNames] = SortFiles(sInputs, avgtype)
    GroupComments = [];
    GroupNames = {};
    % Group files in different ways: by subject, by condition, or all together
    switch (avgtype)
        % === EVERYTHING ===
        case 1
            iGroups = {1:length(sInputs)};
            GroupNames{1} = 'All';

        % === BY SUBJECT ===
        case 2
            [uniqueSubj,I,J] = unique({sInputs.SubjectFile});
            iGroups = cell(1, length(uniqueSubj));
            for i = 1:length(uniqueSubj)
                iGroups{i} = find(J == i)';
                GroupNames{i} = sInputs(iGroups{i}(1)).SubjectName;
            end

        % === BY CONDITION ===
        case {3,4}
            % Subject average
            if (avgtype == 3)
                inputCondPath = cell(1,length(sInputs));
                for iInput = 1:length(sInputs)
                    inputCondPath{iInput} = [sInputs(iInput).SubjectName, '/', sInputs(iInput).Condition];
                end
            % Grand average
            else
                inputCondPath = {sInputs.Condition};
            end
            % Process each condition independently
            [uniqueCond,I,J] = unique(inputCondPath);
            iGroups = cell(1, length(uniqueCond));
            GroupComments = cell(1, length(uniqueCond));
            for i = 1:length(uniqueCond)
                iGroups{i} = find(J == i)';
                GroupComments{i} = sInputs(iGroups{i}(1)).Condition;
            end
            GroupNames = uniqueCond;

        % === BY TRIAL GROUPS ===
        case {5,6,7}
            % Get the condition path (SubjectName/Condition/CommentBase) or (Condition/CommentBase) for each input file
            CondPath = cell(1, length(sInputs));
            trialComment = cell(1, length(sInputs));
            for iInput = 1:length(sInputs)
                % Default comment
                trialComment{iInput} = sInputs(iInput).Comment;
                % If results/timefreq and attached to a data file
                if any(strcmpi(sInputs(iInput).FileType, {'results','timefreq'})) && ~isempty(sInputs(iInput).DataFile)
                    switch (file_gettype(sInputs(iInput).DataFile))
                        case 'data'
                            [sStudyAssoc, iStudyAssoc, iFileAssoc] = bst_get('DataFile', sInputs(iInput).DataFile);
                            if ~isempty(sStudyAssoc)
                                trialComment{iInput} = sStudyAssoc.Data(iFileAssoc).Comment;
                            else
                                bst_report('Warning', 'process_average', sInputs(iInput), ['File skipped, the parent node has been deleted:' 10 sInputs(iInput).DataFile]);
                            end

                        case {'results', 'link'}
                            [sStudyAssoc, iStudyAssoc, iFileAssoc] = bst_get('ResultsFile', sInputs(iInput).DataFile);
                            if ~isempty(sStudyAssoc)
                                [sStudyAssoc2, iStudyAssoc2, iFileAssoc2] = bst_get('DataFile', sStudyAssoc.Result(iFileAssoc).DataFile);
                                if ~isempty(sStudyAssoc2)
                                    trialComment{iInput} = sStudyAssoc2.Data(iFileAssoc2).Comment;
                                else
                                    bst_report('Warning', 'process_average', sInputs(iInput), ['File skipped, the parent node has been deleted:' 10 sStudyAssoc.Result(iFileAssoc).DataFile]);
                                end
                            else
                                bst_report('Warning', 'process_average', sInputs(iInput), ['File skipped, the parent node has been deleted:' 10 sInputs(iInput).DataFile]);
                            end

                        case 'matrix'
                            [sStudyAssoc, iStudyAssoc, iFileAssoc] = bst_get('MatrixFile', sInputs(iInput).DataFile);
                            if ~isempty(sStudyAssoc)
                                trialComment{iInput} = sStudyAssoc.Matrix(iFileAssoc).Comment;
                            else
                                bst_report('Warning', 'process_average', sInputs(iInput), ['File skipped, the parent node has been deleted:' 10 sInputs(iInput).DataFile]);
                            end
                    end
                end
                % Condition average
                if (avgtype == 5)
                    CondPath{iInput} = [sInputs(iInput).SubjectName, '/', sInputs(iInput).Condition, '/', str_remove_parenth(trialComment{iInput})];
                % Subject average
                elseif (avgtype == 6)
                    CondPath{iInput} = [sInputs(iInput).SubjectName, '/', str_remove_parenth(trialComment{iInput})];
                % Grand average
                else
                    CondPath{iInput} = str_remove_parenth(trialComment{iInput});
                end
            end

            % Process each condition independently
            [uniquePath,I,J] = unique(CondPath);
            iGroups = cell(1, length(uniquePath));
            GroupComments = cell(1, length(uniquePath));
            for i = 1:length(uniquePath)
                % Skip empty paths
                if isempty(uniquePath{i})
                    continue;
                end
                % Find files in this condition
                iGroups{i} = find(J == i)';
                GroupComments{i} = str_remove_parenth(trialComment{iGroups{i}(1)});
            end
            GroupNames = uniquePath;
    end
end

%% ===== AVERAGE FILES =====
function OutputFile = AverageFiles(sProcess, sInputs, KeepEvents, isScaleDspm, isWeighted, isMatchRows, isZeroBad)
    OutputFile = [];
    % Parse inputs
    if (nargin < 7) || isempty(isZeroBad)
        isZeroBad = 1;
    end
    if (nargin < 6) || isempty(isMatchRows)
        isMatchRows = 1;
    end
    if (nargin < 5) || isempty(isWeighted)
        isWeighted = 0;
    end
    if (nargin < 4) || isempty(isScaleDspm)
        isScaleDspm = 0;
    end
    if (nargin < 3) || isempty(KeepEvents)
        KeepEvents = 0;
    end

    % === PROCESS AVERAGE ===
    % Get function
    isResults = strcmpi(sInputs(1).FileType, 'results');
    % if isResults && isfield(sProcess.options, 'avg_func')
    if isfield(sProcess.options, 'avg_func') && isfield(sProcess.options.avg_func, 'Value') && ~isempty(sProcess.options.avg_func.Value)
        switch (sProcess.options.avg_func.Value)
            case 1,  Function = 'mean';   isVariance = 0;   strComment = 'Avg';
            case 2,  Function = 'abs';    isVariance = 0;   strComment = 'Avg(abs)';
            case 3,  Function = 'rms';    isVariance = 0;   strComment = 'RMS';
            case 4,  Function = 'mean';   isVariance = 1;   strComment = 'Std';
            case 5,  Function = 'mean';   isVariance = 1;   strComment = 'StdError';
            case 6,  Function = 'mean';   isVariance = 1;   strComment = 'AvgStd';
            case 7,  Function = 'mean';   isVariance = 1;   strComment = 'AvgStderr';
            case 8,  Function = 'median'; isVariance = 0;   strComment = 'Median';
        end
    else
        Function = 'mean';   isVariance = 0;   strComment = 'Avg';
    end
    % Compute average
    [Stat, Messages, iAvgFile, Events] = bst_avg_files({sInputs.FileName}, [], Function, isVariance, isWeighted, isMatchRows, isZeroBad, 1);

    % Apply corrections on the variance value
    if strcmpi(strComment, 'Std') || strcmpi(strComment, 'AvgStd')
        Stat.var = sqrt(Stat.var);
    elseif strcmpi(strComment, 'StdError') ||  strcmpi(strComment, 'AvgStderr')
        Stat.var = sqrt(Stat.var / length(sInputs));
    end
    % Add messages to report
    if ~isempty(Messages)
        if isempty(Stat)
            bst_report('Error', sProcess, sInputs, Messages);
            return;
        else
            bst_report('Warning', sProcess, sInputs, Messages);
        end
    end

    % Load first file of the list
    [sMat, matName] = in_bst(sInputs(iAvgFile(1)).FileName);

    % === SCALE dSPM VALUES (DEPRECATED AFTER INVERSE 2018) ===
    % Apply a scaling to the dSPM functions, to compensate for the fact that the scaling applied to the NoiseCov was not correct
    if isScaleDspm && isResults && isfield(sMat, 'Function') && ismember(sMat.Function, {'dspm','mnp','glsp','lcmvp'}) && isfield(sMat, 'ImageGridAmp')
        if ~isWeighted
            bst_report('Warning', sProcess, [], 'You cannot scale the normalized maps if you do not compute a weighted average. Select the option "Weighted" to enable this option.');
        elseif (sMat.nAvg ~= Stat.nAvg)
            Factor = sqrt(Stat.nAvg) / sqrt(sMat.nAvg);
            bst_report('Warning', sProcess, [], sprintf('Averaging normalized maps (%s): scaling the values by %1.3f to match the number of trials averaged (%d => %d)', sMat.Function, Factor, sMat.nAvg, Stat.nAvg));
            % Apply on both .var and .mean fields
            if isfield(Stat, 'mean') && ~isempty(Stat.mean)
                Stat.mean = Factor * Stat.mean;
            end
            if isfield(Stat, 'var') && ~isempty(Stat.var)
                Stat.var = (Factor ^ 2) * Stat.var;
            end
        end
        % disp('BST> Warning: Averaging dSPM maps is different from computing the dSPM of the average. ');
        % disp('BST>          => dSPM(Average) = sqrt(N) * Average(dSPM)');
    end

    % === CREATE OUTPUT STRUCTURE ===
    % Get output study
    [sStudy, iStudy, Comment, uniqueDataFile] = bst_process('GetOutputStudy', sProcess, sInputs);
    % Comment: forced in the options
    if isfield(sProcess.options, 'Comment') && isfield(sProcess.options.Comment, 'Value') && ~isempty(sProcess.options.Comment.Value)
        Comment = [strComment ': ' sProcess.options.Comment.Value, ' (' num2str(length(sInputs)) ' files)'];
    % Comment: Process default
    else
        % % Replace the number of files
        % Comment = strrep(Comment, [num2str(length(sInputs)) ' '], [num2str(length(iAvgFile)) ' ']);
        % % Add file name
        % Comment = [strComment ': ' Comment, ' (' num2str(length(sInputs)) ' files)'];
        Comment = [strComment ': ' Comment];
    end
    % Weighted
    if isWeighted && ~strcmpi(strComment, 'Median')
        Comment = ['W' Comment];
    end
    % Copy fields from Stat structure
    switch (strComment)
        case {'Avg', 'Avg(abs)', 'RMS', 'Median'}
            sMat.(matName) = Stat.mean;
        case {'Std', 'StdError'}
            sMat.(matName) = Stat.var;
        case {'AvgStd', 'AvgStderr'}
            sMat.(matName) = Stat.mean;
            sMat.Std = Stat.var;
    end
    sMat.ChannelFlag = Stat.ChannelFlag;
    sMat.Time        = Stat.Time;
    sMat.nAvg        = Stat.nAvg;
    sMat.Leff        = Stat.Leff;
    sMat.Comment     = Comment;

    % History: Average
    if isfield(sMat, 'History')
        % Copy the history of the first file (but remove the entries "import_epoch" and "import_time")
        prevHistory = sMat.History;
        if ~isempty(prevHistory)
            % Remove entry 'import_epoch'
            iLineEpoch = find(strcmpi(prevHistory(:,2), 'import_epoch'));
            if ~isempty(iLineEpoch)
                prevHistory(iLineEpoch,:) = [];
            end
            % Remove entry 'import_time'
            iLineTime  = find(strcmpi(prevHistory(:,2), 'import_time'));
            if ~isempty(iLineTime)
                prevHistory(iLineTime,:) = [];
            end
        end
        % History for the new average file
        sMat = bst_history('reset', sMat);
        sMat = bst_history('add', sMat, 'average', FormatComment(sProcess));
        sMat = bst_history('add', sMat, 'average', 'History of the first file:');
        sMat = bst_history('add', sMat, prevHistory, ' - ');
    else
        sMat = bst_history('add', sMat, 'average', Comment);
    end
    % History: Number of files averaged for each channel
    if any(Stat.nGoodSamples(1) ~= Stat.nGoodSamples) && isfield(Stat, 'RowNames') && ~isempty(Stat.RowNames) && iscell(Stat.RowNames) && (length(Stat.RefRowNames) <= 1)
        strInfo = 'Number of files that were averaged, for each signal:';
        sMat = bst_history('add', sMat, 'average', strInfo);
        uniqueGood = unique(Stat.nGoodSamples);
        for i = length(uniqueGood):-1:1
            strHistoryGood = [sprintf(' - %d files: ', uniqueGood(i)), sprintf('%s ', Stat.RowNames{Stat.nGoodSamples == uniqueGood(i)})];
            sMat = bst_history('add', sMat, 'average', strHistoryGood);
            strInfo = [strInfo, 10, strHistoryGood];
        end
        % Generate process warning
        bst_report('Warning', sProcess, sInputs, strInfo);
    end
    % History: List files
    sMat = bst_history('add', sMat, 'average', 'List of averaged files:');
    for i = 1:length(iAvgFile)
        sMat = bst_history('add', sMat, 'average', [' - ' sInputs(iAvgFile(i)).FileName]);
    end

    % Averaging results from the different data file: reset the "DataFile" field
    if isfield(sMat, 'DataFile') && ~isempty(sMat.DataFile) && (length(uniqueDataFile) ~= 1)
        sMat.DataFile = [];
    end
    % Copy all the events found in the input files
    if KeepEvents && ~isempty(Events)
        sMat.Events = Events;
    else
        sMat.Events = [];
    end
    % Rownames
    if isfield(Stat, 'RowNames') && ~isempty(Stat.RowNames)
        if strcmpi(matName, 'TF')
            sMat.RowNames = Stat.RowNames;
        elseif strcmpi(matName, 'Value')
            sMat.Description = Stat.RowNames;
        end
    end

    % === AVERAGE WARPED BRAINS ===
    if isResults
        sMat = FixWarpedSurfaceFile(sMat, sInputs(1), sStudy);
    end

    % === SAVE FILE ===
    % Output filename
    if strcmpi(sInputs(1).FileType, 'data')
        allFiles = {};
        for i = 1:length(sInputs)
            [tmp, allFiles{end+1}, tmp] = bst_fileparts(sInputs(i).FileName);
        end
        fileTag = str_common_path(allFiles, '_');
    else
        fileTag = bst_process('GetFileTag', sInputs(1).FileName);
    end
    OutputFile = bst_process('GetNewFilename', bst_fileparts(sStudy.FileName), [fileTag, '_average']);
    % Save on disk
    bst_save(OutputFile, sMat, 'v6');
    % Register in database
    db_add_data(iStudy, OutputFile, sMat);
end

%% ===== FIX SURFACE FOR WARPED BRAINS =====
% Average source files coming from different subjects that are all different deformations of the default brain:
% should re-use the initial cortex surface instead of the first cortex surface available
function [sMat, isFixed] = FixWarpedSurfaceFile(sMat, sInput, sStudyDest)
    isFixed = 0;
    % Not a warped surface: skip
    if ~isfield(sMat, 'SurfaceFile') || isempty(sMat.SurfaceFile) || isempty(strfind(sMat.SurfaceFile, '_warped'))
        return;
    end
    % Must be from non-default to default anatomy
    isDestDefaultSubj = ismember(bst_fileparts(sStudyDest.BrainStormSubject), {bst_get('DirDefaultSubject'), bst_get('NormalizedSubjectName')});
    isSrcDefaultSubj  = ismember(bst_fileparts(sInput.SubjectFile),           {bst_get('DirDefaultSubject'), bst_get('NormalizedSubjectName')});
    if isSrcDefaultSubj || ~isDestDefaultSubj
        return;
    end
    % Rebuild possible target surface
    [tmp, fBase, fExt] = bst_fileparts(sMat.SurfaceFile);
    SurfaceFileDest = [bst_get('DirDefaultSubject'), '/', strrep([fBase, fExt], '_warped', '')];
    % Find destination file in database
    [sSubjectDest, iSubjectDes, iSurfDest] = bst_get('SurfaceFile', SurfaceFileDest);
    if isempty(iSurfDest)
        return;
    end
    % If this surface exists: use it if it has the same number of vertices as the source surface
    % Get the vertices number from source and destination surface files
    VarInfoSrc  = whos('-file', file_fullpath(sMat.SurfaceFile), 'Vertices');
    VarInfoDest = whos('-file', file_fullpath(SurfaceFileDest), 'Vertices');
    % Number of vertices match: change the surface
    if (VarInfoSrc.size(1) == VarInfoDest.size(1))
        sMat.SurfaceFile = SurfaceFileDest;
        isFixed = 1;
    end
end


function LU = extract_bounds_from_norm_operator(LU,extstruct,extvariables,i);
arg = extstruct(i).arg{1};
if min(size(arg))==1 & length(extstruct(i).arg)>1
    p = extstruct(i).arg{2};
    % Maximize each element conservatively, and use the norm of
    % that worst-case vector
    U = max(abs(LU(getvariables(arg),:)),[],2);
    if ~all(isinf(U))
        xmax = abs(getbase(arg))*[1;U];
        LU(extvariables(i),2) = min([norm(xmax,p) LU(extvariables(i),2)]);
    end
end
% norms are positive...
LU(extvariables(i),1) = max([0 LU(extvariables(i),1)]);


function varargout = spy(X,variablesonly)
%SPY (overloaded)

if nargin < 2
    variablesonly = 0;
end
 if isa(X,'blkvar')
    X = sdpvar(X);
 end

if variablesonly
    Z = X.basis(:,2:end);
else
    Z = X.basis;
end

Z = reshape(sum(abs(Z),2),X.dim(1),X.dim(2));
Z = Z~=0;
if nargout==0
    spy(Z)
else
    varargout{1}=Z;
end


%DYADIC Dyadic datafile operations
%
%  C = DYADIC(A,P,B,Q)
%
% Computes C = P*A + Q*B
%
% This datafile operation is, like others, either stored as
% a preprocessing or as a postprocessing for datafiles using
% a call to DYADICM.
%
% Note that in P a function name can be stored and in Q a cell
% array with a set of parameters. In that case effectively
% feval(p,a1,a2,q{:}) will be executed inside DYADICM.
%
% SEE ALSO (PRTools Guide)
% DATAFILES, DYADICM


function lam = prod_lambda_msgs(n, cs, msg, msg_type, except)

if nargin < 5, except = -1; end

lam = msg{n}.lambda_from_self;
switch msg_type
  case 'd',
   for i=1:length(cs)
     c = cs(i);
     if c ~= except
       lam = lam .* msg{n}.lambda_from_child{i};
     end
   end
 case 'g',
  if isinf(lam.precision) % isfield(lam, 'observed_val')
    return; % pass on the observed msg
  end
   for i=1:length(cs)
     c = cs(i);
     if c ~= except
       m = msg{n}.lambda_from_child{i};
       lam.precision = lam.precision + m.precision;
       lam.info_state = lam.info_state + m.info_state;
     end
   end
end


function [varargout]=quiverCurve(varargin)

%% Parse input

switch nargin
    case 1
        Vg=varargin{1};
        dirOpt=1;
        colorOpt='k';
        alphaLevel=1;
    case 2
        Vg=varargin{1};
        dirOpt=varargin{2};
        colorOpt='k';
        alphaLevel=1;
    case 3
        Vg=varargin{1};
        dirOpt=varargin{2};
        colorOpt=varargin{3};
        alphaLevel=1;
    case 4
        Vg=varargin{1};
        dirOpt=varargin{2};
        colorOpt=varargin{3};
        alphaLevel=varargin{4};
end

%%

if size(Vg,2)==2
    Vg(:,3)=0;
end

switch dirOpt
    case 1 %Forward
        V=Vg(1:end-1,:);
        U=diff(Vg,1,1);
    case 2 %Backward
        V=Vg(2:end,:);
        U=flipud(diff(flipud(Vg),1,1));
    case 3 %Central
        V=Vg;
        Uf=[diff(Vg,1,1); nan(1,size(Vg,2))];
        Ub=-[nan(1,size(Vg,2)); flipud(diff(flipud(Vg),1,1))];
        U=Uf;
        U(:,:,2)=Ub;
        U=gnanmean(U,3);
end
D=sqrt(sum(U.^2,2));
a=[min(D(:)) max(D(:))];

[F,V,C]=quiver3Dpatch(V(:,1),V(:,2),V(:,3),U(:,1),U(:,2),U(:,3),[],a);

if isempty(colorOpt) %If empty use colormapping
    h=gpatch(F,V,C,'none',alphaLevel);
else %else use specified which could be 'k'
    h=gpatch(F,V,colorOpt,'none',alphaLevel);
end

varargout{1}=h;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function van_der_corput_seed_set ( seed )

%*****************************************************************************80
%
%% VAN_DER_CORPUT_SEED_SET sets the seed of the van der Corput sequence.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Johannes van der Corput,
%    Verteilungsfunktionen I & II,
%    Nederl. Akad. Wetensch. Proc.,
%    Volume 38, 1935, pages 813-820, pages 1058-1066.
%
%  Parameters:
%
%    Input, integer SEED, the new value for the seed.  SEED should be
%    nonnegative.  Only the integer part of SEED is used.  SEED = 0
%    is allowed, and returns R = 0.
%
%    Global, integer VAN_DER_CORPUT_SEED, the current seed.
%
  global van_der_corput_BASE
  global van_der_corput_SEED
%
  if ( seed < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'VAN_DER_CORPUT_SEED_SET - Fatal error!\n' );
    fprintf ( 1, '  Input value of SEED < 0!\n' );
    fprintf ( 1, '  SEED = %d\n', seed );
    return
  end

  van_der_corput_SEED = floor ( seed );

  return
end


function update_editfield_date(src,~)
    %update_editfield_date general function to update x.Value with a datenum when x.String is updated, where x is an editfield
    % use as the callback
    %
    %  initialdate = '2015-01-23 14:31:55.01'
    %  uicontrol('Style','edit','string',initialdate,'Value',datenum(initialdate),'Callback',@update_editfield_date);
    % can interpret as decimal year or full date

    isTextdate = contains(src.String,{':',' ','/','-'});

    if isTextdate
        src.Value = datenum(datetime(src.String)); %provides extra parsing
    else
        d = str2double(get(src,'String'));
        if d >= 1800 && d < 3000
            %treat as year
            set(src,'String',[get(src,'String'), '-01-01 00:00:00']);
            update_editfield_date(src);
            return
        else
            try
                src.Value=str2double(src.String);
            catch ME
                warning(ME)
            end
        end
    end
end


function [V,F] = png2mesh( ...
  filename, laplacian_smoothness_iterations, max_points_on_boundary)
  % PNG2MESH
  %
  % [V,F] = png2mesh(filename, laplacian_smoothness_iterations,
  % max_points_on_boundary)
  %
  % Mesh the non transparent part of an image. Best to have the entire shape
  % surrounded by at least one transparent pixel. Also the outer boundary and
  % boundary of holes should be roughly the same size as resampling to meet the
  % max_points_on_boundary parameter is done by edge collapsing on the boundary
  % (small boundaries will end up with too many samples and therefor the
  % triangulation will be dense there).
  %
  % Inputs:
  %   filename  path to .png file
  %   laplacian_smoothness_iterations  Number of iterations of laplacian
  %     smoothing of the positions of boundary curve points before handing
  %     curve to Triangle
  %   max_points_on_boundary  maximum number of points in the input curve
  %     before handing to Triangle
  % Outputs:
  %   F  #faces by 3, list of face indices
  %   V  #V by 2 list of polygon vertices, note that y-coordinates will be
  %     "flipped" with respect to the image if you think of the image rows as
  %     counting from 0 at the top left corner to size(im,1) at the bottom
  %     right corner
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)
  %
  % See also: bwmesh, png2poly, triangle
  %

  warning('obsolete: use bwmesh');

  basename = regexprep(filename,'\.png$','');
  % use alpha channel of png image to define poly
  [V,E,H] = png2poly(...
    filename,...
    laplacian_smoothness_iterations, ...
    max_points_on_boundary);
  if ~isempty(H)
    warning('Holes non-empty, but I know holes sometimes come out broken');
  end

  unr = setdiff(1:size(V,1),E(:));
  if(~isempty(unr))
    warning('Unreferenced vertices in outline...\n');
  end
  % get average squared edge length as a guess at the maximum area constraint
  % for the triangulation
  avg_sqr_edge_length = mean(sum((V(E(:,1),:)-V(E(:,2),:)).^2,2));
  % triangulate the polygon
  [V,F] = triangle(V,E,H,'MaxArea',avg_sqr_edge_length/2.0,'Quality',30);
  %[V,F] = triangle(V,E,[]);
  unr = setdiff(1:size(V,1),F(:));
  if(~isempty(unr))
    warning('Removing unreferenced vertices in mesh...');
    [V,~,F] = faces_first(V,[],F);
    V=V(1:max(F(:)),:);
  end

end


function g = restrict(f, s)
%RESTRICT   Restrict a BNDFUN to a subinterval.
%   RESTRICT(F, S) returns a BNDFUN that is restricted to the subinterval
%   [S(1), S(2)] of F.domain.
%
%   If LENGTH(S) > 2, i.e., S = [S1, S2, S3, ...], then RESTRICT(F, S) returns
%   a cell-array of BNDFUN objects, where the cells contain F restricted to each
%   of the subintervals defined by S.

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

% Deal with empty case:
if ( isempty(f) )
    g = f;
    return
end

% Check if s is actually a subinterval:
hs = diff(f.domain)*eps; % Some wiggle room.
if ( s(1) < f.domain(1) )
    if ( abs(s(1) - f.domain(1)) < hs )
        s(1) = f.domain(1);
    else
        error('CHEBFUN:BNDFUN:restrict:badInterval', 'Not a valid interval.')
    end
end
if ( s(end) > f.domain(2) )
    if ( abs(s(end) - f.domain(2)) < hs )
        s(end) = f.domain(2);
    else
        error('CHEBFUN:BNDFUN:restrict:badInterval', 'Not a valid interval.')
    end
end
if ( any(diff(s) <= 0) )
    error('CHEBFUN:BNDFUN:restrict:badInterval', 'Not a valid interval.')
end

if ( (numel(s) == 2) && all( s == f.domain) )
    % Nothing to do here!
    g = f;
    return
end

% Compute the breaks in [-1,1] space and restrict the onefun:
t = f.mapping.Inv(s);

% Restrict the ONEFUN field of f.
restrictedOnefuns = restrict(f.onefun, t);

% In case S = [S1, S2], i.e., we are only restricting to one subinterval,
% restrictedOnefuns will be a ONEFUN. In case S = [S1, S2, ..., SN], i.e., we
% are restricting to multiple subinterval, restrictedOnefuns will be a
% cell-array of ONEFUN objects. We need to treat the cases differently, as we
% also want to either return a BNDFUN or cell-array of BNDFUN objects depending
% on how many subintervals we are restricting to.
if ( length(s) == 2 )
    % Only restricting to one subinterval -- return a BNDFUN.

    % Create an empty BNDFUN, and assign fields directly. This is faster than
    % using the BNDFUN constructor.
    g = bndfun();
    g.onefun = restrictedOnefuns;
    g.domain = s;
    g.mapping = bndfun.createMap(s);
else
    % Restricting to multiple subintervals -- return a cell-array of BNDFUN
    % objects.

    % Create a cell to be returned.
    g = cell(1, numel(s) - 1);

    % Create an empty BNDFUN:
    emptyBndfun = bndfun();

    % Loop over each of the new subintervals, make a bndfun with new mapping,
    % and store in the cell returned:
    for k = 1:(numel(s) - 1)
        % Assign fields directly to an empty temporary BNDFUN. This is faster
        % than using the BNDFUN constructor.
        gTemp = emptyBndfun;
        gTemp.onefun = restrictedOnefuns{k};
        gTemp.domain = s(k:k+1);
        gTemp.mapping = bndfun.createMap(s(k:k+1));
        g{k} = gTemp;
    end
end

end


classdef net_model_acp < mp.net_model_ac & mp.form_acp

%   MATPOWER
%   Copyright (c) 2019-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

    properties
        va = [];
        vm = [];
    end

    methods
        function obj = net_model_acp()
            obj@mp.net_model_ac();
            obj.element_classes = ...
                { @mp.nme_bus_acp, @mp.nme_gen_acp, @mp.nme_load_acp, ...
                    @mp.nme_branch_acp, @mp.nme_shunt_acp };

            %% Due to a bug related to inheritance in constructors in
            %% Octave 5.2 and earlier (https://savannah.gnu.org/bugs/?52614),
            %% INIT_SET_TYPES() cannot be called directly in the
            %% MP_IDX_MANAGER constructor, as desired.
            %%
            %% WORKAROUND:  INIT_SET_TYPES() is called explicitly as needed
            %%              (if obj.node is empty) in BUILD() and DISPLAY(),
            %%              after object construction, but before object use.
        end

        function obj = def_set_types(obj)
            def_set_types@mp.net_model_ac(obj);     %% call parent first
            obj.set_types.va = 'VOLTAGE ANG VARS (va)';
            obj.set_types.vm = 'VOLTAGE MAG VARS (vm)';
        end

        function va = initial_voltage_angle(obj, idx)
            va = obj.params_var('va');
            if nargin > 1 && ~isempty(idx)
                va = va(idx);
            end
        end
    end     %% methods
end         %% classdef


function [F_maxSize, F_maxLoc] = getFeatMatHeatmap( jobID, taskID, objIDs, CONTIG_THR )
% Identify features covering a contig block w/ length at least CONTIG_THR
% Input:
%    jobID : integer id of job
%    taskID : integer id of which run of the job to examine
%    objIDs : vector of objects to consider
%               if empty, we look at all objects
%    CONTIG_THR : integer specifying minimum length of a contig block
%                  mocap data recorded at 120fps,
%                  block-averaged so more like 10 fps
%                  thus setting CONTIG_THR=10 means 1 sec
% OUTPUT
%   F_maxSize : N x K matrix
%                  where entry n,k gives integer length of *longest*
%                   contig block assigned to feature k in sequence n
%   F_maxLoc  : N x K matrix
%                  where entry n,k gives timestep id of the start of the
%                    where *longest* contig block of feat k in seq. n

if ~exist( 'CONTIG_THR', 'var' )
    CONTIG_THR = 10;
end

INFO = loadSamplerInfo( jobID, taskID );
Q = loadSamplerOutput( jobID, taskID );

data = INFO.data;

F = Q.Psi(end).F;
stateSeq = Q.Psi(end).stateSeq;

K = size(F,2);
if ~exist( 'objIDs', 'var') || isempty( objIDs )
    objIDs = 1:size(F,1);
end
N = length(objIDs);

F_maxSize = zeros( N, K );
F_maxLoc = zeros( N, K );
for aa = 1:length(objIDs )
    [bSiz,bLoc,bVal] = getContigBlocks( stateSeq( objIDs(aa) ).z );

    us = unique( bVal );
    for uu = 1:length( us )
        occurIDs = find( bVal == us(uu) );

        keepers =  bSiz( occurIDs ) > CONTIG_THR;
        if any( keepers )
            occurIDs = occurIDs(keepers);
            if isempty( occurIDs )
                continue;
            end
            [maxSize,maxID] = max( bSiz( occurIDs ) );
            F_maxSize( aa, us(uu) ) = maxSize;
            F_maxLoc( aa, us(uu) ) = bLoc( occurIDs(maxID) );
        end

    end

end

%           if strcmp( INFO.model.obsModel.type, 'Multinomial' )
%               % Make sure each interval has STIPS in >1/2 of the included tsteps
%               doKeep = true(1, length(occurIDs) );
%               for oo = 1:length( occurIDs )
%                   tstart = bLoc(occurIDs(oo));
%                   tstop  = tstart+bSiz( occurIDs(oo) )-1;
%                   if sum( data_struct(aa).nEmissions(tstart:tstop) ) < length( tstart:tstop )/2
%                       doKeep(oo) = false;
%                   end
%               end
%               occurIDs = occurIDs( doKeep );
%           end


function displaytable(data,headings,labels,cellsize,decimalsize,permutation,cellindices,cellcolors)
%
% Prints out a condensed, legible N-dimensional data table with dimension
% headings and variable cell width and precision.
%
% Entries can be highlighted with color using the cprintf function available
% on Matlab Central.
%
% data is an n-dimensional array
% headings {'Row' 'Column'} is a cell array containing the name of each dimension.
% labels {{1 2 3} {4 5 6}} is a cell array containing lists of values for each dimension.
%   each element of labels can be either:
%     cell array of strings;
%     cell array of numbers;
%     array of numbers.
%
% cellsize (optional, default 6) is the width of each displayed cell
% decimalsize (optional, default 2) is the number of decimal places
%   displayed in each cell
%
% permutation [3 4 1 2] describes the ordering of dimensions in the final
%   displayed table. Data, headings, and labels are all permuted accordingly
%
% cellindices [m x ndim]: m n-dimensional tuplets containing the indices of
%   entries to be highlighted
% cellcolors [m x 3]: [R,G,B] color tuplets for entries to be highlighted
%
% Matt Caywood
%
%   Versions:
% 0.1:  2010.06.16 Initial version
% 0.2:  2010.06.29 Fixed degenerate tables with dimension values = 1
% 0.3:  2010.07.15 Added convenient table permutations
% 0.31: 2010.07.16 Fixed dimension error checking
% 0.4:  2010.07.20 Added color highlighting of entries using cprintf
% 0.41: 2010.07.21 Fixed header spacing
% 0.42: 2011.03.08 Worked around -0 issue in printf

% displaytable(rand(5,5),{'1' '2'},{{'C1' 'C2' 'C3' 'I1' 'I2'},{'C1' 'C2' 'C3' 'I1' 'I2'}})

global cellwidth decimalplaces cellformat myprintf;

%% check input valid
if ~isnumeric(data), error('displaytable:NotNumeric','Data must be numeric.'); end
if ~isreal(data), error('displaytable:NotReal','Data must contain only real elements.'); end

% use headings as canonical length, because matlab ignores singleton trailing dimensions in data
nd = length(headings);
dims = size(data);

if any(dims == 0), fprintf('[]\n'); return; end % some dimension is zero
if ndims(data) > nd, error('displaytable:headings','Not enough headings for data.'); end
if length(labels) ~= nd, error('displaytable:headings','Headings and labels are different lengths.'); end
for i = 1:nd
    % accept singleton trailing dimensions
    if ~( (i > length(dims) && 1 == length(labels{i})) || (dims(i) == length(labels{i})) )
       error('displaytable:labels','Label list #%d (%s) is wrong length for data.',i,headings{i});
    end
end

%% create cell format string
if ~exist('cellsize','var'), cellwidth = 6; else cellwidth = cellsize; end
if ~exist('decimalsize', 'var'), decimalplaces = 2; else decimalplaces = decimalsize; end

assert(cellwidth > decimalplaces+1);
cellformat = ['%' int2str(cellwidth-(decimalplaces+1)) '.' int2str(decimalplaces) 'f'];

%% normalize labels to cell array of strings
for i = 1:nd
    if ~iscell(labels{i}), labels{i} = num2cell(labels{i}); end
    for j = 1:length(labels{i})
        if isnumeric(labels{i}{j}), labels{i}{j} = num2str(labels{i}{j}); end
    end
end

%% Use cprintf if available and cellindices specified, otherwise builtin fprintf
myprintf = @(col,fmt,varargin) fprintf(fmt,varargin{:});

if ~exist('cellindices','var')
    cellindices = [];
    cellcolors = [];
else
    assert(size(cellindices,2) == nd,'Cellindices have wrong dimensions.');
    if ~exist('cprintf','file')
        fprintf('WARNING: cprintf not found, colors will not be shown!\nPlease download cprintf from Matlab Central:\nhttp://www.mathworks.com/matlabcentral/fileexchange/24093\n');
    else
        myprintf = @cprintf;
        if ~exist('cellcolors','var')
            cellcolors = repmat([1 0 0],[size(cellindices,1) 1]); % default red
        else
            assert(size(cellcolors,2) == 3,'Colors must be RGB triplets');
            assert(size(cellcolors,1) == size(cellindices,1),'Colors must be equal length to indices');
        end
    end
end

%% permute table
if exist('permutation','var') && ~isempty(permutation)
    assert(length(permutation) == nd,'Wrong permutation length.');
    data = permute(data,permutation);
    headings = {headings{permutation}};
    labels = {labels{permutation}};

    if ~isempty(cellindices)
        for m = 1:size(cellindices,1)
            cellindices(m,:) = cellindices(m,permutation);
        end
    end
end

% recurse to print out all tables
displaytable_helper(data,headings,labels,cellindices,cellcolors);

end

%% recursion helper
function displaytable_helper(data,headings,labels,cellindices,cellcolors)

global myprintf;

nd = ndims(data); % nd always >= 2

if (nd == 2) % 1D row or column vector, or 2D matrix
    display2dtable(data,headings,labels,cellindices,cellcolors);

else % nd > 2
    title = headings{1};
    sublabels = labels{1};
    labelchars = maxchars(sublabels);

    nidx = size(data,1);
    remainingdims = size(data); remainingdims = remainingdims(2:nd);
    for subscr = 1:nidx

        whichidx = cell(1,nd);
        whichidx{1} = subscr;
        for i = 2:nd; whichidx{i} = ':'; end
        subs.type = '()';
        subs.subs = whichidx;
        subtab = subsref(data,subs);
        subtab = reshape(subtab,remainingdims);

        myprintf('text','%s = %s\t\n',title,truncpadstring(sublabels{subscr},labelchars));
        if isempty(cellindices), passindices = [];
        else passindices = (cellindices(:,1) == subscr);
        end
        displaytable_helper(subtab,headings(2:end),labels(2:end),cellindices(passindices,2:end),cellcolors(passindices,:));
    end
end
end

%% base case
function display2dtable(data,headings,labels,cellindices,cellcolors)

assert(~isempty(headings));
assert(nargin >= 3,'Need row, column labels');
assert(nargin == ndims(data) + 3,'Wrong # arguments');

[nrow,ncol] = size(data);

global cellformat cellwidth decimalplaces myprintf;

if length(headings) == 1
    % handle degenerate 1 x n, n x 1 cases
    if (ncol == 1)
        colheading = '';
        collabels = cell(size(data));
        rowheading = headings{1};
        rowlabels = labels{1};
    elseif (nrow == 1)
        colheading = headings{1};
        collabels = labels{1};
        rowheading = '';
        rowlabels = cell(size(data));
    end
else
    rowheading = headings{1};
    rowlabels = labels{1};
    colheading = headings{2};
    collabels = labels{2};
end

rowchars = max(3,maxchars(rowlabels));

% squeeze dimension label into row label space
dimchars = max(1,floor(rowchars/2));
dim = [truncpadstring(rowheading,dimchars) '/' truncpadstring(colheading,dimchars)];

% display header
myprintf('text',dim);
for i = 1:ncol
    myprintf('text',' %s',truncpadstring(collabels{i},cellwidth));
end
myprintf('text','\n');

% display rows
for j = 1:nrow
    myprintf('text',truncpadstring(rowlabels{j},rowchars));
    for i = 1:ncol

        if isempty(cellindices)
            idx = [];
        else
            idx = find(cellindices(:,1) == j & cellindices(:,2) == i);
            if ~isempty(idx), idx = idx(1); end % use first only
        end

        % clean up the rounding a bit so "-0.00" never appears, only "0.00"
        roundeddata = fround(data(j,i),decimalplaces);
        if (roundeddata == 0), roundeddata = 0; end % this gets rid of floating point -0

        if isempty(idx)
            myprintf('text',' %s',truncpadstring(sprintf(cellformat,roundeddata),cellwidth));
        else
            myprintf(cellcolors(idx,:),' %s',truncpadstring(sprintf(cellformat,roundeddata),cellwidth));
        end
    end
    myprintf('text','\n');
end

end

%% -----------------
function n = maxchars(c)
% find maximum length in characters of strings in cell array

n = 0;
for i = 1:length(c)
    n = max(n,length(c{i}));
end

end

%% -----------------
function ps = truncpadstring(s,n)
% fit string s into n characters

if length(s) > n
    ps = s(1:n);
else
    ps = sprintf(['%' num2str(n) 's'],s);
end

end

%% -----------------
function b = fround(a,n)
% fix-round

if ~exist('n','var'), n = 0; end

b = round(a*10^n)/10^n;

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% This files simplifies the generation of FAIR data
% Input
%    example        the name of the game (for figures etc)
%    fileT        name of a image file for the template  image
%    fileR        name of a image file for the reference image
%    varargin    list of additional variables such as omega, m, LM
%==============================================================================

function expfile = jpgs2data(example,fileT,fileR,varargin)

if nargin == 0,
  help(mfilename)
  runMinimalExample
  return;
end

omega     = [];
m         = [];
LM        = [];
viewPara  = {'viewImage','viewImage2D','colormap','gray(256)'};
imgPara   = {'imgModel','linearInter'};
overwrite = 0;

for k=1:2:length(varargin), % overwrite defaults
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

if isempty(example),
  caller  = dbstack;
  expfile = caller(min(2,length(caller))).name;
  example = expfile(6:end-5);
else
  expfile = sprintf('setup%sData',example);
end
expfile = fullfile(FAIRpath,'temp',expfile);

FAIRmessage(expfile)
if exist([expfile,'.mat'],'file') & ~overwrite,
  fprintf('[%s] %s exists\n',mfilename,expfile);
  return
end;

% do whatever needed to be done to get your data here
image = @(str) double(flipud(imread(str))'); % reads and converts

% load the original data, set domain, initial discretization, and grid
dataT = image(fileT);
dataR = image(fileR);

if isempty(omega), omega = [0,size(dataT,1),0,size(dataT,2)];  end;
if isempty(m),     m     = size(dataR);                        end;

viewImage('reset',viewPara{:});
imgModel('reset',imgPara{:});

omegaI = @(i) omega(min(i,size(omega,1)),:);
xc     = @(k) getCellCenteredGrid(omegaI(k),m);

viewData  = @(I,k) viewImage(imgModel(I,omegaI(k),xc(k)),omegaI(k),m);

FAIRfigure(1,'figname',expfile); clf;
subplot(1,2,1); viewData(dataT,1); title('template');
subplot(1,2,2); viewData(dataR,2); title('reference');

ML = getMultilevel({dataT,dataR},omega,m,'fig',2);

% save to outfile
save(expfile,'example','dataT','dataR','omega','m','ML','viewPara','imgPara');
if ~isempty(LM),
  save(expfile,'-append','LM','example');
end;

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

function runMinimalExample
setup2DhandData;

%==============================================================================


function t = sensitivityinfo(fid,seekset,channum)
% T = SENSITIVITYINFO(FID,SEEKSET,CHANNUM);
%   FID = valid filepointer to a sqdfile/ sqd-filename
%   SEEKSET = starting point of file read
%   CHANNUM = Channelnumber(s)
% Gets the info regarding senstivity of sensors from the file pointed
% to by fid for given channel_number and returns a structure

if nargin<1
    error('First argument must be valid file-pointer to a sqd-file');
elseif nargin<2
    seekset = -1;
end;
filename = '';
if ischar(fid)
    filename = fid;
    fid = fopen(filename,'rb','l');
end;

if fid~=-1
    % Get channelcount
    fseek( fid, 16, seekset);
    basic_offset = fread(fid,1,'long')+(3*32+8*128+8*128)/8;
    fseek( fid, basic_offset, seekset );
    channelcount	= fread(fid,1,'int');
    if nargin<3
        channum = 0:channelcount-1;
    end;
    if (~isempty(find(channum<0)))|(~isempty(find(channum>channelcount-1)))
        error([ 'Sorry - Invalid channel number, ' num2str(channum) ', given to sensorinfo']);
    end;
    % Get offset of sensitivity values
    fseek( fid, 80, seekset );
    sensitivity_offset = fread(fid,1,'long');
    % Read sensitivity data
    fseek( fid, sensitivity_offset, seekset ); %sizeof(double) = 8 bytes and read 2 doubles
    data = fread(fid,channelcount*2,'double');
    t.Offsets  = [data(channum*2+1)]';
%     t.Gains    = [data(channum*2+2)*10^12]';
    t.Gains    = [data(channum*2+2)*10^15]';
    if ~isempty(filename)
        fclose(fid);
    end;
else
    error('Invalid filename');
end;


function varargout = cutMeshByPlane(v, f, plane, varargin)
%CUTMESHBYPLANE Cut a mesh by a plane.
%
%   [ABOVE, IN, BELOW] = cutMeshByPlane(MESH, PLANE)
%   where MESH, ABOVE, IN, BELOW are structs with the fields vertices and
%   faces, and PLANE is given as a row containing initial point and 2
%   direction vectors. ABOVE, IN, BELOW contain the corresponding parts of
%   the input mesh.
%
%   [ABOVE_V, ABOVE_F, IN_V, IN_F, BELOW_V,BELOW_F] = ...
%       cutMeshByPlane(V, F, PLANE) where V is a [NVx3] array containing
%   coordinates and F is a [NFx3] array containing indices of vertices of
%   the triangular faces.
%
%   BELOW = cutMeshByPlane(V, F, PLANE, 'part', 'below') BELOW is a struct
%   with the fields vertices and faces. Other options are:
%       'part'  -   'above': Faces above the plane
%               -   'in'   : Faces in the plane
%               -   'below': Faces below the plane
%
%   [BELOW_V, BELOW_F] = cutMeshByPlane(MESH, PLANE, 'part', 'below') is
%   possible, too.
%

% ---------
% Authors: oqilipo, David Legland
% Created: 2017-07-09
% Copyright 2017

narginchk(2,5)
nargoutchk(1,6)

%% Parse inputs
% If first argument is a struct
if nargin == 2 || nargin == 4
    if ~isempty(varargin)
        varargin = [{plane}, varargin(:)'];
    end
    plane = f;
    [v, f] = parseMeshData(v);
end

p = inputParser;
addRequired(p,'plane',@isPlane)
validStrings = {'above','in','below'};
addParameter(p,'part','above',@(x) any(validatestring(x, validStrings)))
parse(p, plane, varargin{:});
part=p.Results.part;

%% Algorithm
% Logical index to the vertices below the plane
VBPl_LI = isBelowPlane(v, plane);

% Logical index to three vertices of each face
FBP_LI = VBPl_LI(f);
switch nargout
    case {1, 2}
        switch part
            case 'above'
                % Faces above the plane, all three vertices == 0 -> sum has to be 0
                above = removeMeshFaces(v, f, ~(sum(FBP_LI, 2) == 0) );
            case 'in'
                % Faces in the plane, 1 or 2 vertices == 0 -> sum can be 1 or 2
                inside = removeMeshFaces(v, f, ~((sum(FBP_LI, 2) > 0 & sum(FBP_LI, 2) < 3)));
            case 'below'
                % Faces below the plane, all three vertices == 1 -> sum has to be 3
                below = removeMeshFaces(v, f, ~(sum(FBP_LI, 2) == 3) );
        end
    case {3, 6}
        % Faces above the plane, all three vertices == 0 -> sum has to be 0
        above = removeMeshFaces(v, f, ~(sum(FBP_LI, 2) == 0) );
        % Faces in the plane, 1 or 2 vertices == 0 -> sum can be 1 or 2
        inside = removeMeshFaces(v, f, ~((sum(FBP_LI, 2) > 0 & sum(FBP_LI, 2) < 3)));
        % Faces below the plane, all three vertices == 1 -> sum has to be 3
        below = removeMeshFaces(v, f, ~(sum(FBP_LI, 2) == 3) );
    otherwise
        error('Invalid number of output arguments')
end

%% Parse outputs
switch nargout
    case 1
        switch part
            case 'above'
                varargout{1}=above;
            case 'in'
                varargout{1}=inside;
            case 'below'
                varargout{1}=below;
        end
    case 2
        switch part
            case 'above'
                varargout{1}=above.vertices;
                varargout{2}=above.faces;
            case 'in'
                varargout{1}=inside.vertices;
                varargout{2}=inside.faces;
            case 'below'
                varargout{1}=below.vertices;
                varargout{2}=below.faces;
        end
    case 3
        varargout{1}=above;
        varargout{2}=inside;
        varargout{3}=below;
    case 6
        varargout{1}=above.vertices;
        varargout{2}=above.faces;
        varargout{3}=inside.vertices;
        varargout{4}=inside.faces;
        varargout{5}=below.vertices;
        varargout{6}=below.faces;
    otherwise
        error('Invalid number of output arguments')
end

end


function [clpot, loglik] = enter_soft_evidence(engine, CPDpot, observed, pot_type, filter)
% ENTER_SOFT_EVIDENCE Add the specified soft evidence to the network (bk)
% [clpot, loglik] = enter_soft_evidence(engine, CPDpot, observed, pot_type, filter)

[ss T] = size(CPDpot);
C = length(engine.clusters);
S = length(engine.separators);
Q = length(cliques_from_engine(engine.sub_engine));
Q1 = length(cliques_from_engine(engine.sub_engine1));
clpot = cell(Q,T);
alpha = cell(C,T);

% Forwards
% The method is a generalization of the following HMM equation:
% alpha(j,t) = normalise( (sum_i alpha(i,t-1) * transmat(i,j)) * obsmat(j,t) )
% where alpha(j,t) = Pr(Q(t)=j | y(1:t))
t = 1;
[clpot(1:Q1,t), logscale(t)] = enter_soft_evidence(engine.sub_engine1, engine.clq_ass_to_node1(:), ...
					   CPDpot(:,1), find(observed(:,1)), pot_type);
for c=1:C
  k = engine.clq_ass_to_cluster1(c);
  alpha{c,t} = marginalize_pot(clpot{k,t}, engine.clusters{c});
end

%=== FH: For each separator s, divide some cluster potential by s's potential
alpha_orig = alpha(:,t);
for s=1:S
  c = engine.cluster_ass_to_separator(s);
  alpha{c,t} = divide_by_pot(alpha{c,t}, marginalize_pot(alpha_orig{c}, engine.separators{s}));
end

% For filtering, clpot{1} contains evidence on slice 1 only

%fprintf('alphas t=%d\n', t);
%for c=1:8
%  temp = pot_to_marginal(alpha{c,t});
%  temp.T
%end

% clpot{t} contains evidence from slices t-1, t for t > 1
clqs = [engine.clq_ass_to_cluster(:,1); engine.clq_ass_to_node(:,2)];
for t=2:T
  pots = [alpha(:,t-1); CPDpot(:,t)];
  [clpot(:,t), logscale(t)] = enter_soft_evidence(engine.sub_engine, clqs, pots, find(observed(:,t-1:t)),  pot_type);
  for c=1:C
    k = engine.clq_ass_to_cluster(c,2);
    cl = engine.clusters{c};
    alpha{c,t} = marginalize_pot(clpot{k,t}, cl+ss); % extract slice 2 posterior
    alpha{c,t} = set_domain_pot(alpha{c,t}, cl); % shift back to slice 1 for re-use as prior
  end
  %=== FH: For each separator s, divide some cluster potential by s's potential
  alpha_orig = alpha(:,t);
  for s=1:S
    c = engine.cluster_ass_to_separator(s);
    alpha{c,t} = divide_by_pot(alpha{c,t}, marginalize_pot(alpha_orig{c}, engine.separators{s}));
  end
end

loglik = sum(logscale);

if filter
  return;
end

% Backwards
% The method is a generalization of the following HMM equation:
% beta(i,t) = (sum_j transmat(i,j) * obsmat(j,t+1) * beta(j,t+1))
% where beta(i,t) = Pr(y(t+1:T) | Q(t)=i)
t = T;
bnet = bnet_from_engine(engine);
beta = cell(C,T);
for c=1:C
  beta{c,t} = mk_initial_pot(pot_type, engine.clusters{c} + ss, bnet.node_sizes(:), bnet.cnodes(:), ...
			     find(observed(:,t-1:t)));
end
%=== FH: For each separator s, divide some cluster potential by s's potential
beta_orig = beta(:,t);
for s=1:S
  c = engine.cluster_ass_to_separator(s);
  beta{c,t} = divide_by_pot(beta{c,t}, marginalize_pot(beta_orig{c}, engine.separators{s}+ss));
end

for t=T-1:-1:1
  clqs = [engine.clq_ass_to_cluster(:,2); engine.clq_ass_to_node(:,2)];
  pots = [beta(:,t+1); CPDpot(:,t+1)];
  temp = enter_soft_evidence(engine.sub_engine, clqs, pots, find(observed(:,t:t+1)),  pot_type);
  for c=1:C
    k = engine.clq_ass_to_cluster(c,1);
    cl = engine.clusters{c};
    beta{c,t} = marginalize_pot(temp{k}, cl); % extract slice 1
    beta{c,t} = set_domain_pot(beta{c,t}, cl + ss); % shift fwd to slice 2
  end
  %=== FH: For each separator s, divide some cluster potential by s's potential
  beta_orig = beta(:,t);
  for s=1:S
    c = engine.cluster_ass_to_separator(s);
    beta{c,t} = divide_by_pot(beta{c,t}, marginalize_pot(beta_orig{c}, engine.separators{s}+ss));
  end
end

% Combine
% The method is a generalization of the following HMM equation:
% xi(i,j,t) = normalise( alpha(i,t) * transmat(i,j) * obsmat(j,t+1) * beta(j,t+1) )
% where xi(i,j,t) = Pr(Q(t)=i, Q(t+1)=j | y(1:T))
for t=1:T-1
  clqs = [engine.clq_ass_to_cluster(:); engine.clq_ass_to_node(:,2)];
  pots = [alpha(:,t); beta(:,t+1); CPDpot(:,t+1)];
  clpot(:,t+1) = enter_soft_evidence(engine.sub_engine, clqs, pots, find(observed(:,t:t+1)),  pot_type);
end
% for smoothing, clpot{1} is undefined
for k=1:Q1
  clpot{k,1} = [];
end


function test_bug2370

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_filetype ft_read_headshape ft_write_headshape read_ply write_ply

filename = [tempname '.ply'];

[pos, tri] = mesh_sphere(162);

% combine them in a structure
mesh1.pos = pos;
mesh1.tri = tri;

ft_write_headshape(filename, mesh1, 'format', 'ply');
mesh2 = ft_read_headshape(filename);
assert(isequal(mesh1.tri, mesh2.tri));
ratio = mesh1.pos(:)\mesh2.pos(:);
assert(ratio>0.99 && ratio<1.01);
delete(filename);

ft_write_headshape(filename, mesh1, 'format', 'ply_ascii');
mesh2 = ft_read_headshape(filename);
assert(isequal(mesh1.tri, mesh2.tri));
ratio = mesh1.pos(:)\mesh2.pos(:);
assert(ratio>0.99 && ratio<1.01);
delete(filename);

ft_write_headshape(filename, mesh1, 'format', 'ply_binary');
mesh2 = ft_read_headshape(filename);
assert(isequal(mesh1.tri, mesh2.tri));
ratio = mesh1.pos(:)\mesh2.pos(:);
assert(ratio>0.99 && ratio<1.01);
delete(filename);


% center distance to closest larva
function [data,units] = compute_dcentral(trx,n)

larvae = trx.exp2flies{n};
nlarvae = numel(larvae);
data = cell(1,nlarvae);

for i1 = 1:nlarvae,
  larva1 = larvae(i1);
  % access closestlarva to ensure that dcentral is computed
  trx(larva1).closestlarva_central;
  data{i1} = trx(larva1).dcentral;
end
units = parseunits('mm');


% LFUnpackRawBuffer - Unpack a buffer of packed raw binary data into an image
%
% Usage:
%
%     ImgOut = LFUnpackRawBuffer( Buff, BitPacking, ImgSize )
%
% Used by LFReadRaw and LFReadLFP, this helper function unpacks a raw binary data buffer in one of several formats.
%
% Inputs :
%
%     Buff : Buffer of chars to be unpacked
%     BitPacking : one of '12bit', '10bit' or '16bit'; default is '12bit'
%     ImgSize : size of the output image
%
% Outputs:
%
%      Img : an array of uint16 gray levels. No demosaicing (decoding Bayer pattern) is performed.
%
% User guide: LFToolbox.pdf
% See also: LFDecodeLensletImageDirect, demosaic

% Copyright (c) 2013-2020 Donald G. Dansereau

function ImgOut = LFUnpackRawBuffer( Buff, BitPacking, ImgSize )

switch( BitPacking )
    case '8bit'
        ImgOut = reshape(Buff, ImgSize);

    case '10bit'
        t0 = uint16(Buff(1:5:end));
        t1 = uint16(Buff(2:5:end));
        t2 = uint16(Buff(3:5:end));
        t3 = uint16(Buff(4:5:end));
        lsb = uint16(Buff(5:5:end));

        t0 = bitshift(t0,2);
        t1 = bitshift(t1,2);
        t2 = bitshift(t2,2);
        t3 = bitshift(t3,2);

        t0 = t0 + bitand(lsb,bin2dec('00000011'));
        t1 = t1 + bitshift(bitand(lsb,bin2dec('00001100')),-2);
        t2 = t2 + bitshift(bitand(lsb,bin2dec('00110000')),-4);
        t3 = t3 + bitshift(bitand(lsb,bin2dec('11000000')),-6);

        ImgOut = zeros(ImgSize, 'uint16');
        ImgOut(1:4:end) = t0;
        ImgOut(2:4:end) = t1;
        ImgOut(3:4:end) = t2;
        ImgOut(4:4:end) = t3;

    case '12bit'
        t0 = uint16(Buff(1:3:end));
        t1 = uint16(Buff(2:3:end));
        t2 = uint16(Buff(3:3:end));

        a0 = bitshift(t0,4) + bitshift(bitand(t1,bin2dec('11110000')),-4);
        a1 = bitshift(bitand(t1,bin2dec('00001111')),8) + t2;

        ImgOut = zeros(ImgSize, 'uint16');
        ImgOut(1:2:end) = a0;
        ImgOut(2:2:end) = a1;

    case '16bit'
        t0 = uint16(Buff(1:2:end));
        t1 = uint16(Buff(2:2:end));
        a0 = bitshift(t1, 8) + t0;
        ImgOut = zeros(ImgSize, 'uint16');
        ImgOut(:) = a0;

    otherwise
        error('Unrecognized bit packing');
end


function [fP] = callback_GetPercentile(fPercentile, vDistribution, fValue)
    % get percentile
    fP = abs(prctile(vDistribution, fPercentile) - fValue);
end


function analyse_eval_lin_fusion(key_dev,ndx_eval,dev_scores_obj_array,eval_scores_obj_array,dev_scr_lin,eval_scr_lin)
% Does some analyses on evaluation scores for all systems and all
% fusions.  For each system, it calculates the estimated percentage
% of targets in the database.  It also calculates a KL divergence
% matrix of all systems against all systems.
% Inputs:
%   key_dev: The Key for the dev trials.
%   ndx_eval: The Ndx describing the eval trials.
%   dev_scores_obj_array: An array of Scores objects containing
%     system scores for the dev trials.
%   eval_scores_obj_array: An array of Scores objects containing
%     system scores for the eval trials.
%   dev_scr_lin: The dev scores resulting from the linear fusion.
%   eval_scr_lin: The eval scores resulting from the linear fusion.

assert(nargin==6)
assert(isa(key_dev,'Key'))
assert(isa(ndx_eval,'Ndx'))
assert(isa(dev_scr_lin,'Scores'))
assert(isa(eval_scr_lin,'Scores'))
assert(key_dev.validate())
assert(ndx_eval.validate())
assert(dev_scr_lin.validate())
assert(eval_scr_lin.validate())

eval_stacked_scores = stackScores(ndx_eval,eval_scores_obj_array);
trialmask = ndx_eval.trialmask;
eval_lin_fusion = eval_scr_lin.scoremat(trialmask(:))';
eval_scores = [eval_stacked_scores;eval_lin_fusion];

ptar = estimate_target_proportions(eval_scores,0.08);
logprint(Logger.Info,'\nSystem Ptar estimates:\n');
logprint(Logger.Info,'%s',sprintfmatrix(100*ptar'));

ptar_best = ptar(end);

dev_stacked_scores = stackScores(key_dev,dev_scores_obj_array);
trialmask = key_dev.to_ndx().trialmask;
dev_lin_fusion = dev_scr_lin.scoremat(trialmask(:))';
dev_scores = [dev_stacked_scores;dev_lin_fusion];

devKL = 100*kldivergence_of_systems(dev_scores,ptar_best,key_dev);
evalKL = 100*kldivergence_of_systems(eval_scores,ptar_best);
logprint(Logger.Info,'\nDev score divergence:\n%s',sprintfmatrix(devKL));
logprint(Logger.Info,'\nEval score divergence:\n%s',sprintfmatrix(evalKL));
end


function calpak_test39 ( )

%*****************************************************************************80
%
%% CALPAK_TEST39 tests MONTH_TO_MONTH_NAME_COMMON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST39\n' );
  fprintf ( 1, '  For the Common calendar,\n' );
  fprintf ( 1, '  MONTH_TO_MONTH_NAME_COMMON names the months:\n' );
  fprintf ( 1, '\n' );

  y = 1;
  months = year_length_months_common ( y );

  for m = 1 : months
    month_name = month_to_month_name_common ( m );
    fprintf ( 1, '  %2d  %s\n', m, month_name );
  end

  return
end


function [ clmParams, pdm_right, pdm_left ] = Load_CLM_params_eye()
%LOAD_CLM_PARAMS_WILD Summary of this function goes here
%   Detailed explanation goes here
    clmParams.window_size = [15,15; 13,13;];
    clmParams.numPatchIters = size(clmParams.window_size,1);

    % the PDM created from in the wild data
    pdmLoc = ['../models/hierarch_pdm/pdm_6_r_eye.mat'];

    load(pdmLoc);

    pdm_right = struct;
    pdm_right.M = double(M);
    pdm_right.E = double(E);
    pdm_right.V = double(V);

    pdmLoc = ['../models/hierarch_pdm/pdm_6_l_eye.mat'];

    load(pdmLoc);

    pdm_left = struct;
    pdm_left.M = double(M);
    pdm_left.E = double(E);
    pdm_left.V = double(V);

    % the default model parameters to use
    clmParams.regFactor = 0.1;
    clmParams.sigmaMeanShift = 2;
    clmParams.tikhonov_factor = 0;

    clmParams.startScale = 1;
    clmParams.num_RLMS_iter = 5;
    clmParams.fTol = 0.01;
    clmParams.useMultiScale = true;
    clmParams.use_multi_modal = 1;
    clmParams.tikhonov_factor = 0;
end


function [VV,FF,J] = extrude(V,F,varargin)
  % EXTRUDE Extrude a 2d mesh in the z direction by 1, connecting boundaries
  % apropriately
  %
  % [VV,FF] = extrude(V,F)
  % [VV,FF,J] = extrude(V,F,'ParmeterName',ParameterValue,...)
  %
  % Inputs:
  %   V  #V by 2 list of 2d vertex positions
  %   F  #F by 3 list of triangle indices into V
  %     or
  %   E  #E by 2 list of segment indices into V, CCW order
  %   Optional:
  %     'Cap' followed by {true} of false whether to put a *bottom cap*
  %     'Levels'  followed by number of "stacks" or "levels" {1}
  % Outputs:
  %   VV #VV by 3 list of 3d vertex positions
  %   FF  #FF by 3 list of triangle indices into VV
  %   J  #VV indices into V
  %

  cap = [];
  levels = 1;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Cap','Levels'}, ...
    {'cap','levels'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isempty(cap)
    cap = size(F,2) > 2;
  end

  %assert(size(V,2) == 2, 'Vertices should be 2d');

  %% Copy top and bottom
  %  VV = [V 1+0*V(:,1);V 0*V(:,1)];
  switch size(F,2)
  case {3,4}
    % connect boundaries
    O = outline(F);
  case 2
    assert(~cap,'Cap should be false when input is curve');
    % Curve as input
    FF = [];
    O = F;
    % Use triangles
    F = zeros(0,3);
  end
  assert(size(unique(sort(O,2),'rows'),1) == size(O,1));

  % Rearrange vertices on bottom
  [VB,FB,OB,IM] = faces_first(V,F,O);
  n = size(V,1);
  JB = (1:n)';

  no = numel(unique(OB));

  VL = [ ...
      repmat(VB(1:max(OB(:)),:),[levels 1]) ...
        reshape(repmat(linspace(1-1/levels,0,levels),[no,1]),[no*levels 1])];
  JL = repmat((1:no)',levels,1);
  if cap
    VV = [VB ones(n,1);VL(1:no*(levels-1),:);VB zeros(n,1)];
    J = [JB;JL(1:no*(levels-1));JB];
  else
    VV = [VB ones(n,1);VL];
    J = [JB;JL];
  end
  FO = ones(2*size(OB,1)*levels,size(F,2));
  for level = 1:levels
    if level == 1
      off_t = 0;
    else
      off_t = n+(level-2)*no;
    end
    off_b = n+(level-1)*no;
    switch size(F,2)
    case 4
      FO((level-1)*size(OB,1)+(1:size(OB,1)),:) = [ ...
        off_t+OB(:,[2 1]) off_b+OB(:,1:2)];
    case 3
      FO((level-1)*2*size(OB,1)+(1:size(OB,1)*2),:) = [ ...
        off_b+OB(:,1) off_t+OB(:,[2 1]); ...
        off_t+OB(:,2) off_b+OB(:,1:2)];
    end
  end

  % remap so that faces on planes are consistent with input
  if cap
    FF = [FB;FO;n+(levels-1)*no+fliplr(FB)];
    RIM = 1:(n+(levels-1)*no+n);
    RIM(IM) = 1:n;
    RIM(n+IM) = n+(1:n);
  else
    FF = [FB;FO];
    RIM = 1:(n+levels*no);
    RIM(IM) = 1:n;
  end

  VV(RIM,:) = VV;
  J(RIM,:) = RIM(J);
  FF = RIM(FF);

end


function check_point = terminate(xi_1,xi,pi_1,pi)

temp1 = max(abs(xi_1(1,:)-xi(1,:))) + max(abs(pi_1(1,:)-pi(1,:)));
temp2 = max(abs(xi_1(2,:)-xi(2,:))) + max(abs(pi_1(2,:)-pi(2,:)));

check_point  = temp1+temp2;


function st = spm_cfg_st
% SPM Configuration file for Slice Timing Correction
%__________________________________________________________________________
% Copyright (C) 2005-2016 Wellcome Trust Centre for Neuroimaging

% $Id: spm_cfg_st.m 7591 2019-05-15 10:57:38Z john $

%--------------------------------------------------------------------------
% scans Session
%--------------------------------------------------------------------------
scans         = cfg_files;
scans.tag     = 'scans';
scans.name    = 'Session';
scans.help    = {'Select images to slice-time correct.'};
scans.filter  = 'image';
scans.ufilter = '.*';
scans.num     = [1 Inf];
scans.preview = @(f) spm_check_registration(char(f));

%--------------------------------------------------------------------------
% generic Data
%--------------------------------------------------------------------------
generic         = cfg_repeat;
generic.tag     = 'generic';
generic.name    = 'Data';
generic.help    = {'Subjects or sessions. The same parameters specified below will be applied to all sessions.'};
generic.values  = {scans };
generic.num     = [1 Inf];

%--------------------------------------------------------------------------
% nslices Number of Slices
%--------------------------------------------------------------------------
nslices         = cfg_entry;
nslices.tag     = 'nslices';
nslices.name    = 'Number of Slices';
nslices.help    = {'Enter the number of slices.'};
nslices.strtype = 'n';
nslices.num     = [1 1];

%--------------------------------------------------------------------------
% tr TR
%--------------------------------------------------------------------------
tr         = cfg_entry;
tr.tag     = 'tr';
tr.name    = 'TR';
tr.help    = {'Enter the TR (in seconds).'};
tr.strtype = 'r';
tr.num     = [1 1];

%--------------------------------------------------------------------------
% ta TA
%--------------------------------------------------------------------------
ta         = cfg_entry;
ta.tag     = 'ta';
ta.name    = 'TA';
ta.help    = {
    'Enter the TA (in seconds). It is usually calculated as TR-(TR/nslices).'
    ''
    'You can simply enter this equation with the variables replaced by appropriate numbers.'
    ''
    'If the next two items are entered in milliseconds, this entry will not be used and can be set to 0.'
    }';
ta.strtype = 'r';
ta.num     = [1 1];

%--------------------------------------------------------------------------
% so Slice order
%--------------------------------------------------------------------------
so         = cfg_entry;
so.tag     = 'so';
so.name    = 'Slice order';
so.help    = {
    'Enter the slice order.'
    ''
    'Bottom slice = 1. Sequence types and examples of code to enter are given below:'
    ''
    '  ascending (first slice=bottom): [1:1:nslices]'
    ''
    '  descending (first slice=top): [nslices:-1:1]'
    ''
    '  interleaved (middle-top):'
    '     for k = 1:nslices'
    '         round((nslices-k)/2 + (rem((nslices-k),2) * (nslices - 1)/2)) + 1,'
    '     end'
    ''
    '  interleaved (bottom -> up): [1:2:nslices 2:2:nslices]'
    ''
    '  interleaved (top -> down): [nslices:-2:1, nslices-1:-2:1]'
    ''
    'Alternatively you can enter the slice timing in ms for each slice individually.'
    'If doing so, the next item (Reference Slice) will contain a reference time (in ms) instead of the slice index of the reference slice.'
    'For Siemens scanners, this can be acquired in MATLAB from the dicom header as follows (use any volume after the first one):'
    '   hdr = spm_dicom_headers(''dicom.ima'');'
    '   slice_times = hdr{1}.Private_0019_1029'
    'Note that slice ordering is assumed to be from foot to head. If it is not, enter instead: TR - INTRASCAN_TIME - SLICE_TIMING_VECTOR'
    }';
so.strtype = 'r';
so.num     = [1 Inf];

%--------------------------------------------------------------------------
% refslice Reference Slice
%--------------------------------------------------------------------------
refslice         = cfg_entry;
refslice.tag     = 'refslice';
refslice.name    = 'Reference Slice';
refslice.help    = {'Enter the reference slice.'
                    ''
                    'If slice times are provided instead of slice indices in the previous item, this value should represent a reference time (in ms) instead of the slice index of the reference slice.'};
refslice.strtype = 'r';
refslice.num     = [1 1];

%--------------------------------------------------------------------------
% prefix Filename Prefix
%--------------------------------------------------------------------------
prefix         = cfg_entry;
prefix.tag     = 'prefix';
prefix.name    = 'Filename Prefix';
prefix.help    = {'Specify the string to be prepended to the filenames of the slice-time corrected image file(s).','Default prefix is ''a''.'};
prefix.strtype = 's';
prefix.num     = [1 Inf];
prefix.def     = @(val)spm_get_defaults('slicetiming.prefix', val{:});

%--------------------------------------------------------------------------
% st Slice Timing
%--------------------------------------------------------------------------
st         = cfg_exbranch;
st.tag     = 'st';
st.name    = 'Slice Timing';
st.val     = {generic nslices tr ta so refslice prefix};
st.help    = {
    'Correct differences in image acquisition time between slices.'
    ''
    'Slice-time corrected files are prepended with an ''a''.'
    ''
    'Note: The sliceorder arg that specifies slice acquisition order is a vector of N numbers, where N is the number of slices per volume. Each number refers to the position of a slice within the image file. The order of numbers within the vector is the temporal order in which those slices were acquired. To check the order of slices within an image file, use the SPM Display option and move the cross-hairs to a voxel co-ordinate of z=1.  This corresponds to a point in the first slice of the volume.'
    ''
    'The function corrects differences in slice acquisition times. This routine is intended to correct for the staggered order of slice acquisition that is used during echo-planar scanning. The correction is necessary to make the data on each slice correspond to the same point in time. Without correction, the data on one slice will represent a point in time as far removed as 1/2 the TR from an adjacent slice (in the case of an interleaved sequence).'
    ''
    'This routine "shifts" a signal in time to provide an output vector that represents the same (continuous) signal sampled starting either later or earlier. This is accomplished by a simple shift of the phase of the sines that make up the signal. Recall that a Fourier transform allows for a representation of any signal as the linear combination of sinusoids of different frequencies and phases. Effectively, we will add a constant to the phase of every frequency, shifting the data in time.'
    ''
    'Shifter - This is the filter by which the signal will be convolved to introduce the phase shift. It is constructed explicitly in the Fourier domain. In the time domain, it may be described as an impulse (delta function) that has been shifted in time the amount described by TimeShift. The correction works by lagging (shifting forward) the time-series data on each slice using sinc-interpolation. This results in each time series having the values that would have been obtained had the slice been acquired at the same time as the reference slice. To make this clear, consider a neural event (and ensuing hemodynamic response) that occurs simultaneously on two adjacent slices. Values from slice "A" are acquired starting at time zero, simultaneous to the neural event, while values from slice "B" are acquired one second later. Without correction, the "B" values will describe a hemodynamic response that will appear to have began one second EARLIER on the "B" slice than on slice "A". To correct for this, the "B" values need to be shifted towards the Right, i.e., towards the last value.'
    ''
    'This correction assumes that the data are band-limited (i.e. there is no meaningful information present in the data at a frequency higher than that of the Nyquist). This assumption is support by the study of Josephs et al (1997, Human Brain Mapping)/* \cite{josephs1997event} */ that obtained event-related data at an effective TR of 166 msecs. No physio-logical signal change was present at frequencies higher than our typical Nyquist (0.25 HZ).'
    ''
    'When using the slice timing correction it is very important that you input the correct slice order, and if there is any uncertainty then users are encouraged to work with their physicist to determine the actual slice acquisition order.'
    ''
    'One can also consider augmenting the model by including the temporal derivative in the informed basis set instead of slice timing, which can account for +/- 1 second of changes in timing.'
    ''
    'Written by Darren Gitelman at Northwestern U., 1998.  Based (in large part) on ACQCORRECT.PRO from Geoff Aguirre and Eric Zarahn at U. Penn.'
    }';
st.prog = @spm_run_st;
st.vout = @vout;
st.modality = {'FMRI'};

%==========================================================================
function dep = vout(job)
for k=1:numel(job.scans)
    dep(k)            = cfg_dep;
    dep(k).sname      = sprintf('Slice Timing Corr. Images (Sess %d)', k);
    dep(k).src_output = substruct('()',{k}, '.','files');
    dep(k).tgt_spec   = cfg_findspec({{'filter','image','strtype','e'}});
end


function model_3d = train_nrsfm(cls,train_set)
    globals;
    params = get_params();
    md = isnan(train_set.points(1:end/2,:));
    points = train_set.points;
    points_bak = points;
    points(isnan(points))=0;
    try
        seg.poly_x = train_set.poly_x;
        seg.poly_y = train_set.poly_y;
    catch
        seg = train_set.mask;
    end

    % Setup variables for converting to PASCAL 3D frame
    load(fullfile(datadir,'partNames',cls));
    load(fullfile(datadir,'voc_kp_metadata'));
    cInd = find(ismember(metadata.categories,cls));
    [~,I] = sort(metadata.kp_names{cInd});
    invI(I) = 1:numel(I);
    rightCoordSys = metadata.right_coordinate_sys{cInd};
    rightCoordSys(1:6) = invI(rightCoordSys(:));
    names = partNames(rightCoordSys);

    % Do non-rigid SFM
    [P3, S_bar, V, RO, Tr, Z, sigma_sq, ~, ~, ~, ~,c]...
      = em_sfm(points, md, params.nrsfm.nBasis, seg, train_set.imsize, train_set.bbox,...
      params.nrsfm.tol, params.nrsfm.max_em_iter,rightCoordSys,train_set.rotP3d);

    model_3d.class          = cls;
    model_3d.S_bar          = S_bar;
    model_3d.defBasis       = V;
    model_3d.part_names     = train_set.labels;
    model_3d.sigma_sq       = sigma_sq;
    model_3d.points3        = P3;
    model_3d.rots           = RO;
    model_3d.trs            = Tr;
    model_3d.def_weights    = Z;
    model_3d.voc_image_id   = train_set.voc_image_id;
    if(isfield(train_set,'voc_rec_id'))
        model_3d.voc_rec_id     = train_set.voc_rec_id;
    end
    model_3d.bbox           = train_set.bbox;
    model_3d.points2        = points_bak;
    model_3d.params         = params;
    model_3d.c              = c;
    model_3d.seg            = seg;
    if(isfield(train_set,'flip'))
        model_3d.flip = train_set.flip;
    end
    if(isfield(train_set,'subtype'))
        model_3d.subtype = train_set.subtype;
    end
    if(isfield(train_set,'rotP3d'))
        model_3d.rotP3d = train_set.rotP3d;
    end

end


function value = hammersley_base_check ( dim_num, base )

%*****************************************************************************80
%
%% HAMMERSLEY_BASE_CHECK checks BASE for a Hammersley sequence.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer BASE(1:DIM_NUM), the bases.
%    Bases should not be 0 or 1.
%
%    Output, logical VALUE, is true if BASE is legal.
%
  if ( any ( base(1:dim_num) == 0 ) | any ( base(1:dim_num) == 1 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HAMMERSLEY_BASE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  BASE(I) = 0 or 1 for some I!\n' );
    value = 0;
  else
    value = 1;
  end

  return
end


function CPD = maximize_params(CPD, temp)
% MAXIMIZE_PARAMS Set the params of a tabular node to their ML/MAP values.
% CPD = maximize_params(CPD, temp)

if ~adjustable_CPD(CPD), return; end

%assert(approxeq(sum(CPD.counts(:)), CPD.nsamples)); % false!
switch CPD.prior_type
 case 'none',
  counts = reshape(CPD.counts, size(CPD.CPT));
  CPD.CPT = mk_stochastic(counts);
 case 'dirichlet',
  counts = reshape(CPD.counts, size(CPD.CPT));
  CPD.CPT = mk_stochastic(counts + CPD.dirichlet);

 % case 'entropic',
%   % For an HMM,
%   % CPT(i,j) = pr(X(t)=j | X(t-1)=i) = transprob(i,j)
%   % counts(i,j) = E #(X(t-1)=i, X(t)=j) = exp_num_trans(i,j)
%   Z = 1-temp;
%   fam_sz = CPD.sizes;
%   psz = prod(fam_sz(1:end-1));
%   ssz = fam_sz(end);
%   counts = reshape(CPD.counts, psz, ssz);
%   CPT = zeros(psz, ssz);
%   for i=CPD.entropic_pcases(:)'
%     [CPT(i,:), logpost] = entropic_map_estimate(counts(i,:), Z);
%   end
%   non_entropic_pcases = mysetdiff(1:psz, CPD.entropic_pcases);
%   for i=non_entropic_pcases(:)'
%     CPT(i,:) = mk_stochastic(counts(i,:));
%   end
%   %for i=1:psz
%   %  [CPT(i,:), logpost] = entropic_map(counts(i,:), Z);
%   %end
%   if CPD.trim & (temp < 2) % at high temps, we would trim everything!
%     % grad(j) = d log lik / d theta(i ->j)
%     % CPT(i,j) = 0 => counts(i,j) = 0
%     % so we can safely replace 0s by 1s in the denominator
%     denom = CPT(i,:) + (CPT(i,:)==0);
%     grad = counts(i,:) ./ denom;
%     trim = find(CPT(i,:) <= exp(-(1/Z)*grad)); % eqn 32
%     if ~isempty(trim)
%       CPT(i,trim) = 0;
%       if all(CPD.trimmed_trans(i,trim)==0) % trimming for 1st time
% 	disp(['trimming CPT(' num2str(i) ',' num2str(trim) ')'])
%       end
%       CPD.trimmed_trans(i,trim) = 1;
%     end
%   end
%   CPD.CPT = myreshape(CPT, CPD.sizes);
end


%sim_dMul.m
%Jamie Near, 2020.
%
% USAGE:
% d_out = sim_dMul(d_in,factor)
%
% DESCRIPTION:
% Multiply a density matrix by a scalar.  This function is necessary becuase
% the density matrix is a cell array, so cannot be scaled using simple
% multiplication.
%
% INPUTS:
% d_in      = input density matrix to be multiplied.
% factor    = scalar multiplcation factor.
%
% OUTPUTS:
% d_out     = output, result of d_in * factor.

function d_out = sim_dMul(d_in,factor)

d_out=cell(size(d_in));
for m=1:length(d_in)
    d_out{m}=d_in{m}.*factor;
end


function pos = xyzhumanevaJoint2pos(joint)

% XYZHUMANEVAJOINT2POS
%
% COPYRIGHT : Carl Henrik Ek and Neil Lawrence, 2008

% MOCAP

pos = zeros(1,prod(size(joint)));

pos(1:3:end) = joint(:,1);
pos(2:3:end) = joint(:,2);
pos(3:3:end) = joint(:,3);

return


function [g1, g2] = simwhiteXrbfinfwhiteKernGradient(simKern, rbfKern, t1, varargin)

% SIMWHITEXRBFINFWHITEKERNGRADIENT Compute a cross gradient between a
% SIM-WHITE kernel and an RBF-WHITE kernel (with integration limits between
% minus infinity and infinity).
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between a
% SIM-WHITE and an RBF-WHITE kernels for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% FORMAT
% DESC computes cross kernel terms between a SIM-WHITE kernel and an
% RBF-WHITE kernel (with integration limits between minus infinity and
% infinity) for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, simwhiteKernParamInit,
% rbfinfwhiteKernParamInit, simwhiteKernExtractParam, rbfinfwhiteKernExtractParam
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if simKern.variance ~= rbfKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

g1 = zeros(1, simKern.nParams);
g2 = zeros(1, rbfKern.nParams);

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;

% Parameters required for further computations
isStationary = simKern.isStationary;
variance = simKern.variance;
decay = simKern.decay;
sensitivity = simKern.sensitivity;
invWidth = rbfKern.inverseWidth;

% Setting normalised parameters for computation of K
simKern.variance = 1;
rbfKern.variance = 1;
simKern.sensitivity = 1;
K = simwhiteXrbfinfwhiteKernCompute(simKern, rbfKern, t1, t2);

% Gradient w.r.t. the decay (simKern)
g1(1) = variance * sensitivity * sum(sum( ...
    ((decay/invWidth-deltaT) .* K  + 1/sqrt(2*pi*invWidth) ...
        * (exp(-(decay*T1+0.5*invWidth*(T2.^2))) * (~isStationary) ...
            - exp(-0.5*invWidth*(deltaT.^2)))) ...
    .* covGrad));
%       * exp(0.5*(decay^2)/invWidth-decay*deltaT) ...
%         .* (exp(-0.5*invWidth*((T2+decay/invWidth).^2)) * (~isStationary) ...
%           - exp(-0.5*invWidth*((deltaT-decay/invWidth).^2)))) ...

% Gradient w.r.t. the inverse width (rbfKern)
g2(1) = 0.5 * variance * sensitivity * sum(sum( ...
    ((-(decay/invWidth)^2) * K  + 1/sqrt(2*pi*invWidth) ...
        * ((T2-decay/invWidth) .* exp(-(decay*T1+0.5*invWidth*(T2.^2))) * (~isStationary) ...
            + (deltaT+decay/invWidth) .* exp(-0.5*invWidth*(deltaT.^2)))) ...
    .* covGrad));
%       * exp(0.5*(decay^2)/invWidth - decay*deltaT) ...
%         .* ((T2-decay/invWidth) .* exp(-0.5*invWidth*((T2+decay/invWidth).^2)) * (~isStationary) ...
%           + (deltaT+decay/invWidth) .* exp(-0.5*invWidth*((deltaT-decay/invWidth).^2)))) ...

% Gradient w.r.t. sigma_r^2
g1(2) = sensitivity * sum(sum(K .* covGrad));
g2(2) = 0; % Otherwise it is counted twice

% Gradient w.r.t. sensitivity (only simKern)
g1(3) = variance * sum(sum(K .* covGrad));


clear
close all
clc;

%time
dt=0.01;

%coeffs
Cg=50.;
Cd=.3;
minSpeed=5;

name='greenpyr.tiff'
sanitizedname=[strrep(name,'.','') '_']
mkdir([sanitizedname 'outputdir'])
%green
%greenheight=double(imread('testgreen.tiff'))/256*1.7;
%greenheight=double(imread('greencone.tiff'))/256*.3;
%greenheight=double(imread('greenconeinv.tiff'))/256*.3;
greenheight=fliplr(rot90(rot90(double(imread(name))/256*.4)));
greenheightPLOT=fliplr(rot90(rot90(double(imread(name))/256*.2)));
[gradX,gradY]=gradient(greenheight(:,:,1));
forceFun=@(R,RL) (RL-R)/dt*Cd + [interp2(-gradX,R(1,:),R(2,:));interp2(-gradY,R(1,:),R(2,:))]*Cg;
%forceFun=@(R,RL) [interp2(-gradX,R(1,:),R(2,:));interp2(-gradY,R(1,:),R(2,:))];
l=size(greenheight,1);

%pin
holeLoc=size(greenheight)'/2;
holeRadius=3;

%graphics
%sph=sphere(30)
alt=30.85;
az=-39+90;
cmapgreen=flipud([181,228,138; 160,220,104; 124,216,87; 81,212,76; 52,188,67; 37,167,60; 32,154,61; 1,114,56; 0,86,19]/255);
stickX=[holeLoc(1),holeLoc(1)];
stickY=[holeLoc(2),holeLoc(2)];
stickZ=[greenheight(floor(holeLoc(1)),floor(holeLoc(1))),greenheight(floor(holeLoc(1)),floor(holeLoc(1)))+100];
flagX=[stickX;stickX+40];
flagY=[stickY;stickY];
flagZ=[stickZ(2),stickZ(2)-30;stickZ(2),stickZ(2)-30];
flagC=zeros(2,2,3);
flagC(:,:,1)=.9;

% balls
startLoc=[64;64]*3.5;

%angles=(45)*pi/180;
%speeds=400;
%angles=linspace((45-15)*pi/180,(45+15)*pi/180,10);%-pi/4;
angles=linspace((45-180)*pi/180,(45+180)*pi/180,100);%-pi/4;
speeds=linspace(minSpeed,300,100);

numParticles=length(speeds)*length(angles);

speeds=reshape(speeds,1,1,length(speeds));
angleVectors=[cos(angles);sin(angles)];
startConditions=bsxfun(@times,speeds,angleVectors);
startConditions=reshape(startConditions,2,length(speeds)*length(angles));

r=repmat(startLoc,1,numParticles);
rl=r-startConditions*dt;

rstart=r;
rlstart=rl;

%sequentialhits staging
%each ball laucnhes after 1/4 second (300 iters/4) = 80 ticks
launchTicks=80

i=0;
while sum(sum(r~=rl))
    %energy verlet + drag
    i=i+1;
    rn=2*r-rl+(forceFun(r,rl))*dt^2;
    rl=r;
    r=rn;

    %%%LAUNCH CONDITIONS
    if i<50
        r=rstart;
        rl=rlstart;
    end

    %%%STOP CONDITIONS
    %static friction
    dr=rl-r;
    s=sqrt(dr(1,:).^2+dr(2,:).^2)/dt;
    %walls
    haltBallsEdge=(r>l)|(r<1);
    %in the hole
    distToHole=bsxfun(@minus,holeLoc,rl);%use rl not r to check if the ball is in the hole so it can't escape
    distToHole=sqrt(distToHole(1,:).^2+distToHole(2,:).^2);
    %stop those in stop conditions
    holeBalls=(distToHole<holeRadius);
    rl(:,holeBalls)=repmat(holeLoc,1,sum(holeBalls));
    haltBalls=(haltBallsEdge(1,:)|haltBallsEdge(2,:))|(s<minSpeed)|(distToHole<holeRadius);
    r(:,haltBalls)=rl(:,haltBalls);

    if mod(i,10)==0
        figure(1)
        surf(greenheightPLOT,'edgecolor','none')
        colormap(cmapgreen)
        set(gca,'DataAspectRatio',[1 1 1])
        hold on
        scatter3(r(1,:),r(2,:),3+greenheightPLOT(sub2ind(size(greenheightPLOT), floor(r(2,:)),floor(r(1,:)))),'wo','filled')
        plot3(stickX,stickY,stickZ,'k-')
        surf(flagX,flagY,flagZ,flagC,'edgecolor','none')
        hold off
        az=az+.1;
        view([az alt])
%         imshow(greenheight,[min(min(greenheight)),max(max(greenheight))],'colormap',colormap('parula'))
        hold on
        %rectangle('Position',[holeLoc(1)-holeRadius holeLoc(2)-holeRadius holeRadius*2 holeRadius*2],'Curvature',[1,1],'FaceColor','black');
        %scatter(r(1,:),r(2,:),'w.')
        hold off
        xlim([0,512])
        ylim([0,512])
        %title(num2str(s(1)))
        set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0.04, 1, 0.96]);
        saveas(gcf,[[sanitizedname 'outputdir'] '\' num2str(i/10,'%05i') '.png'])
    end
end

%phase space

figure(2)
maxDist=256;
maxColor=reshape([1,0,0],1,1,3);
minColor=reshape([1,1,1],1,1,3);
dists=fliplr(flipud(reshape(distToHole,length(angles),length(speeds))'));
imshow(dists,[holeRadius,maxDist],'colormap',colormap('jet'))
hold on
h=imshow(repmat(ones(size(dists)),1,1,3));
hold off
set(h,'AlphaData',dists<holeRadius);
ylabel('Putt speed')
xlabel('putt angle')


filename='Bridge_tetrahedra_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.05;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;