function ROC = roc_plot(input_values, binary_outcome, varargin)
% This function makes a specific kind of ROC curve plot, based on input
% values along a continuous distribution and a binary outcome variable
% (logical)
%
% :Usage:
% ::
%
%     ROC = roc_plot(input_values, binary_outcome, ['include', include])
%
% Include is an optional logical variable of cases to include
%
% :Optional Inputs:
%
%   **'include':**
%        followed by logical vector of cases to include
%
%   **'threshold':**
%        followed by a priori threshold cutoff for determining misclassification
%
%   **'threshold_type':**
%        followed by thresh type: choices below:
%          - 'Optimal balanced error rate'
%          - 'Optimal overall accuracy' [default]
%          - 'Minimum SDT bias'
%          - [Enter threshold OR threshold_type]
%
%   **'color':**
%        followed by color, e.g., 'r' or [1 .5 0]
%
%   **'plotmethod':**
%        followed by 'deciles' [default] or 'observed'
%
%   **'nonormfit':**
%        suppress normal curve fitting to ROC
%
%   **'plothistograms':**
%        plot histograms of the signal present/absent distributions
%
%   **'writerscoreplus':**
%        Write text file for input into RScorePlus by Lew Harvey
%
%   **'boot':**
%        [default] Bootstrap 95% confidence intervals for sens, spec, PPV at threshold
%
%   **'noboot':**
%        Skip bootstrap
%
%   **'balanced':**
%        Balanced accuracy for single interval classification
%        THIS IS NOT COMPLETELY IMPLEMENTED BECAUSE IT AFFECTS ACCURACY
%        ESTIMATES, BUT NOT P-VALUES OR THRESHOLD AT WHICH TO EVALUATE SENS/SPEC
%
%   **'dependent':**
%        followed by vector of subject IDs, e.g., ('dependent',[1,1,2,2,3,3].
%
%        This will perform multilevel version of binomial test for single interval classification.
%
%   **'noplot':**
%        Skip generating plots
%
%   **'nooutput':**
%        Suppress text output
%
% :Outputs:
%
%   A structure containing the true and false pos rates (tpr, fpr) along the curve
%   and the criterion threshold values of the input variable (thr) corresponding to these rates.
%
% Sensitivity:                  Chances of predicting a "yes" given true "yes"
% Specificity:                  Chances of predicting a "no" given true "no"
% Positive predictive value:    Chances of true "yes" given predicted "yes"
% Negative predictive value:    Chances of true "no" given predicted "no"
% d or d_a:                     Effect size for classification; higher values indicate stronger predictive signal.
% AUC:                          Area under the ROC curve. Higher values indicate more true signal, with a max of 1
%                               (100% sensitivity with 100% specificity, perfect classification).
% sensitivity_ci, other _ci     95% confidence intervals for sensitivity and other statistics
%
%   Uses the function roc_calc.m
%
%   Also returns some information about misclassified observations
%   and line handle for ROC line plot and other statistics:
%     - area under ROC curve
%     - accuracy statistics based on binomial test
%     - PPV
%
% :Examples:
% ::
%
%    ROC = roc_plot(pattern_exp_values, ishot);
%    ROC = roc_plot(pattern_exp_values, ishot, 'threshold', 2.5);
%    ROC = roc_plot(pattern_exp_values, ishot, 'color', 'r', 'twochoice');
%    ROC = roc_plot(pattern_exp_values, ishot, 'color', 'r', 'twochoice', 'nonormfit');
%    ROC = roc_plot(pexp, logical(outcome), 'color', 'g', 'plothistograms', 'threshold', 0.3188);
%    ROC = roc_plot(pexp, logical(outcome), 'twochoice', 'color', 'b', 'plothistograms');
%    ROC = roc_plot(pexp, logical(outcome), 'writerscoreplus');
%    ROC = roc_plot(pexp, logical(outcome), 'color', 'r', 'plotmethod', 'observed', 'plothistograms');
%    ROC = roc_plot(pexp, logical(outcome), 'color', 'm', 'plotmethod', 'observed', 'plothistograms', 'Optimal overall accuracy');
%
% For a whole image with p-values, this may be helpful.
%
% Pre-specifies p-values you want to evaluate at.
% ::
%    rocout = roc_plot(1-t.p, truevals, 'plotmethod', 'observed', 'valuestoevaluate', 1 - [.5:-.1:.1 .05 .01 .005 .001 .0001 .00001 .000001 .0000001 .00000001]);
%
% ..
%    Tor Wager, Feb 2012
%    See Notes in text for more programming details.
%

%    Notes:
%    Tor Edited 3/17/2012 to add standard Gaussian signal detection fit curves,
%    effect size estimates based on Gaussian equal variance model
%
%    Tor Edited 3/20/2012 to fix AUC estimate and add/change output.
%
%    Tor Tested 3/20/12 against RScorePlus.  There are some consequences of
%    binning for input to RScorePlus, inclding that the "response" criteria are inferred
%    in RScorePlus, but they are given if we are selecting arbitrary criteria based on
%    continuous measures (e.g., brain activity). This influences the actual
%    estimates of the mean and std. signal distributions, as well as the
%    sens/spec estimates within response bins. This function does not use
%    arbitrary bins whenever possible, and uses the full ROC across thresholds
%    corresponding to each unique increment in specificity for AUC
%    calculation.
%
%    Edited 3/11/2014: Luke Chang to add balanced accuracy option for single
%    interval classification when classes are unbalanced
%
%    Edited 6/24/2014: Luke Chang to add multilevel binomial test for single
%    interval classification.  Assumes each subject has equal number of
%    trials.  Requires at least more than 20 subjects to ensure distribution
%    is reasonably approximated by normal distribution.  Uses one sample-test
%    across subjects.
%
%    Edited 1/13/2015: Luke Chang - added option to suppress plots for running
%    on cluster.
%
%    Edited 3/25/2015: Luke Chang - added option to suppress text output for
%    speeding up computations on cluster
%
%    Edited 8/2015: Tor Wager - reduced length of thr to speed computation
%    with large numbers of values (e.g., images with 50K+voxels)
%
%    12/8/2018: Tor Wager - made 2 substantive changes:
%     For 'Optimal balanced error rate', we don't want the threshold
%     that optimizes overall sensitivity+specificity; we want the threshold that
%     minimizes the discrepancy between sensitivity and specificity.
%     This is because with unbalanced classes, one can always choose
%     "yes" or "no" and the threshold will be at the bounds, yielding a
%     sens/spec of 0% and 100% or vice versa.
%
%     Threshold optimization correction for binomial P-values
%     Chance values can be tricky, and optimizing a threshold can be
%     circular. For example, if only 33% of observations are true
%     positives, and the classifier always guesses "no", it will be right
%     66% of the time, which is better than the 50% expected under random guessing.
%     We want to build in a correction for threshold selection.
%     The expectation under chance is that we'll adopt a strategy that
%     gives us at least the base-rate of true positive or true negative
%     results. For 33% true-pos, this would be 66%.
%     We take the max p-value of the difference from 0.5 and the baserate
%     or 1 - baserate.
%     Not implemented for 'dependent' binomial test.
%
% ..

include = true(size(binary_outcome));
threshold_type = 'Optimal overall accuracy';
class_thr = [];
color = [.2 .2 .2];
plotmethod = 'deciles'; %'npoints'; % 'observed';
donormfit = 1;
istwochoice = 0;
reportstats90 = 0;
plothistograms = 0;
writerscoreplus = 0;
doboot = 1;
dobalanced = 0;
doDependent = 0;
doplot = 1;
doOutput = 1;
valuestoevaluate = 'auto';

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            case 'include', include = logical(varargin{i+1});

            case 'threshold'
                class_thr = varargin{i + 1};
                threshold_type = 'A priori threshold';

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

            case {'Optimal overall accuracy', 'Optimal balanced error rate', 'Minimum SDT bias'}
                threshold_type = varargin{i};

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

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

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

            case 'nonormfit'
                donormfit = 0;

            case {'twochoice', 'forcedchoice', 'pairedobservations'}
                istwochoice = 1;

            case 'reportstats90', reportstats90 = 1;

            case 'plothistograms', plothistograms = 1;

            case 'boot', doboot = 1;
            case 'noboot', doboot = 0;

            case 'balanced'
                dobalanced = 1;
                error('THIS OPTION IS NOT IMPLEMENTED CORRECTLY; SEE CODE. CONSIDER USING ''Optimal balanced error rate'' OPTION');

            case 'noplot'
                doplot = 0;

            case 'nooutput'
                doOutput = 0;

            case 'dependent'
                doDependent = 1;
                subject_id = varargin{i + 1};
                if(~ismatrix(subject_id) || ~isnumeric(subject_id) || length(input_values)~=length(subject_id))
                    error('Make sure ''dependent'' flag is followed by valid subject_id vector')
                end

                disp('ROC for single interval classification of paired observations.')

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

if length(include) ~= length(binary_outcome) || ~any(include)
    error('Problem with include variable')
end

input_values = input_values(include);
binary_outcome = logical(binary_outcome(include));

% Deal with paired observations
% -------------------------------------------------------------------------

if istwochoice
    % Adjust input scores to reflect differences from the mean of each pair
    % This allows us to do forced-choice classification for pairs based on
    % which is higher. The higher one will always be above the mean.
    % The threshold used here should be zero.

    if doOutput
        disp('ROC for two-choice classification of paired observations.')
        disp('Assumes pos and null outcome observations have the same subject order.')
        disp('Using a priori threshold of 0 for pairwise differences.')
    end

    meanscores = (input_values(binary_outcome) + input_values(~binary_outcome)) ./ 2;

    input_values(binary_outcome) = input_values(binary_outcome) - meanscores;
    input_values(~binary_outcome) = input_values(~binary_outcome) - meanscores;

    threshold_type = 'A priori threshold';
    class_thr = 0;
end

% Get ROC values and main results
% -------------------------------------------------------------------------
if ischar(valuestoevaluate) && strcmp(valuestoevaluate, 'auto') % default

len = length(binary_outcome);
newlen = min(50*len, 500); % max 500 values to evaluate.

thr = linspace(min(input_values), max(input_values), newlen); %min(input_values):.01:max(input_values);

else
    thr = valuestoevaluate;
end

% AUC will be replaced with theoretical value in 2AFC case!
[dummy, tpr, fpr, auc, c_bias] = roc_calc(input_values, binary_outcome, thr);

% count signal present and signal absent
n1 = sum(~isnan(input_values(binary_outcome)) & ~isinf(input_values(binary_outcome)));
n0 = sum(~isnan(input_values(~binary_outcome)) & ~isinf(input_values(~binary_outcome)));

% Get criterion threshold for final stats output
% -------------------------------------------------------------------------

switch threshold_type
    case 'Optimal balanced error rate'

        % For 'Optimal balanced error rate', we don't want the threshold
        % that optimizes overall sensitivity+specificity; we want the threshold that
        % minimizes the discrepancy between sensitivity and specificity.
        % This is because with unbalanced classes, one can always choose
        % "yes" or "no" and the threshold will be at the bounds, yielding a
        % sens/spec of 0% and 100% or vice versa.

        % old:
%         avg = mean([tpr; 1-fpr]);
%         [dummy, wh] = max(avg);
%         class_thr = thr(wh);

        % new:
        tfdiff = diff([tpr; (1-fpr)]);
        [dummy, wh] = min(abs(tfdiff));
        class_thr = thr(wh);

        dobalanced = 1;

    case 'A priori threshold'

        % Report a priori threshold
        %         wh = find(thr <= class_thr);
        %         wh = wh(end);

    case 'Optimal overall accuracy'

        ncorrt = tpr .* n1;
        ncorrf = (1 - fpr) .* n0;

        mysum = sum([ncorrt; ncorrf]);
        [dummy, wh] = max(mysum);
        class_thr = thr(wh);

    case 'Minimum SDT bias'
        [dummy, wh] = min(abs(c_bias));
        class_thr = thr(wh);

    otherwise
        error('Unknown threshold type')
end

% Save stuff
% -------------------------------------------------------------------------
ROC.baserate = sum(binary_outcome) ./ length(binary_outcome);

ROC.all_vals.thr = thr;

ROC.class_threshold = class_thr;
ROC.sensitivity = sum(input_values(binary_outcome) >= class_thr) ./ n1; % go back to original thresh for precision when using specific input thresh
ROC.specificity = 1 - ( sum(input_values(~binary_outcome) >= class_thr) ./ n0 );
ROC.AUC = auc;
ROC.AUC_descrip = 'Numerically integrated, nonparametric area under curve';

% vectors of true/false positives and accuracy
if istwochoice
    falseneg = (input_values <= class_thr & binary_outcome); % Wani added this line to fix an error when two values are same (in the two-choice test)
else
    falseneg = (input_values < class_thr & binary_outcome);
end
falsepos = (input_values >= class_thr & ~binary_outcome);
misclass =  falseneg | falsepos;
truepos = binary_outcome & ~misclass;
trueneg = ~binary_outcome & ~misclass;

ROC.threshold_type_for_misclass = threshold_type;

if istwochoice
    % Reshape to reflect pairs for stats/output
    sz = [length(misclass) ./ 2 2];

    if any(sz ~= round(sz))
        disp('Two-choice classification assumes you enter paired observations in order:')
        disp('signal present for obs (1:n) followed by signal absent for (1:n) in the same order or vice versa.')
        error('The input has the wrong size.')
    end

    truepos = truepos(binary_outcome);
    trueneg = trueneg(~binary_outcome);
    falseneg = falseneg(binary_outcome);
    falsepos = falsepos(~binary_outcome);
    misclass = falsepos | falseneg;

    if any(truepos & falsepos) || any(trueneg & falseneg)
        disp('Two-choice classification assumes you enter paired observations in order:')
        disp('signal present for obs (1:n) followed by signal absent for (1:n) in the same order or vice versa.')
        error('Inconsistent output: The observations are likely not in order.')
    end

end

% Stuff for figuring out which points are misclassified

ROC.observations.truepos = truepos;
ROC.observations.trueneg = trueneg;
ROC.observations.falseneg = falseneg;
ROC.observations.falsepos = falsepos;
ROC.observations.misclass = misclass;

ROC.PPV = sum(ROC.observations.truepos) ./ (sum(ROC.observations.truepos) + sum(ROC.observations.falsepos));

% Accuracy stats
if ~dobalanced
    accuracy = 1 - (sum(misclass) ./ length(misclass));
else
    accuracy = (sum(truepos)/(sum(truepos) + sum(falseneg)) + sum(trueneg)/(sum(trueneg) + sum(falsepos)))/2;
end

ROC.accuracy = accuracy;

if ~doDependent

    % Threshold optimization correction for binomial P-values
    % Chance values can be tricky, and optimizing a threshold can be
    % circular. For example, if only 33% of observations are true
    % positives, and the classifier always guesses "no", it will be right
    % 66% of the time, which is better than the 50% expected under random guessing.
    % We want to build in a correction for threshold selection.
    % The expectation under chance is that we'll adopt a strategy that
    % gives us at least the base-rate of true positive or true negative
    % results. For 33% true-pos, this would be 66%.
    % We take the max p-value of the difference from 0.5 and the baserate
    % or 1 - baserate.
    % Not implemented for 'dependent' binomial test.

    RES = binotest(double(~misclass), .5);
    RES2 = binotest(double(~misclass), ROC.baserate);
    RES3 = binotest(double(~misclass), 1 - ROC.baserate);
    RES.p_val = max([RES.p_val RES2.p_val RES3.p_val]);

else %Run hierarchical version of binomial test

    %Create new subject matrix
    subID = unique(subject_id);
    for i = 1:length(subID)
        sdat(i,:) = ~misclass(subject_id == subID(i));
    end
    [RES1, RES2, RES3, RES4] = binotest_dependent(sdat,.5);
    RES = RES4;

end
ROC.N = RES.n;
ROC.accuracy_p = RES.p_val;
ROC.accuracy_se = RES.SE;

% Stuff for re-plotting full ROC
ROC.all_vals.tpr = tpr;
ROC.all_vals.fpr = fpr;

% -------------------------------------------------------------------------
% Plot stuff
%
% Get ROC values for plot - default is to plot 10 points
% -------------------------------------------------------------------------

switch plotmethod
    case 'deciles'
        plotthr = prctile(input_values, [10 20 30 40 50 60 70 80 90]);
        [dummy, plottpr, plotfpr] = roc_calc(input_values, binary_outcome, plotthr);
        plotsymbol = 'o';
        linewid = 2;

    case 'observed'
        plotthr = thr;
        plottpr = tpr;
        plotfpr = fpr;
        plotsymbol = '-';
        linewid = 3;
end

if doplot
    han = plot(plotfpr, plottpr, plotsymbol, 'Color', color, 'LineWidth', linewid);

    set(gca, 'FontSize', 32, 'XLim', [-.02 1.02], 'XTick', 0:.2:1, 'YTick', 0:.2:1);
    xlabel('(1 - Specificity)');
    ylabel('Sensitivity')

    inline_plothistograms();
end

% effect size
% -------------------------------------------------------------------------

if istwochoice
    % Two-alternative forced choice
    % -------------------------------------------------------------

    diffscores = input_values(binary_outcome) - input_values(~binary_outcome);

    meandiff = mean(diffscores);
    d = meandiff ./ std(diffscores);

    % forced-choice is variance of the difference, which is 2 * pooled variance
    % std of difference therefore = pooledsd * sqrt(2), and pooledsd = std(diffs)/sqrt(2)
    pooledsd = std(diffscores) ./ sqrt(2);

    d_a_model = meandiff ./ pooledsd; % estimate of what d_a would be for single-interval

    % From Lew Harvey's notes - this should be the "observed" d_a based on
    % empirical accuracy. But it will be inaccurate as accuracy approaches
    % 1. Use "model" because it's closer to the data, no norminv inaccuracy
    d_a_obs = sqrt(2) * norminv(ROC.accuracy);

    if donormfit
        % standard equal-variance signal detection
        x = [-3:.1:3];
        tprn = 1 - normcdf(x, d, 1);
        fprn = 1 - normcdf(x, -d, 1);
        hold on;
        if doplot
            han = [han plot(fprn, tprn, '-', 'Color', color, 'LineWidth', linewid)];
        end
    end

    aucn = calc_auc(fprn, tprn);
    expected_acc = 1 - normcdf(0, d, 1); % expected to be = to the AUC!

    ROC.Gaussian_model.type = 'Two-alternative forced choice';
    ROC.Gaussian_model.names = {'diff_scores'};
    ROC.Gaussian_model.means = meandiff;
    ROC.Gaussian_model.n = length(diffscores);
    ROC.Gaussian_model.sd = std(diffscores);
    ROC.Gaussian_model.d_a = d_a_model;
    ROC.Gaussian_model.d_a_descrip = '''Observed'' d_a based on empirical accuracy.';
    ROC.Gaussian_model.sensitivity = expected_acc;
    ROC.Gaussian_model.specificity = expected_acc;

    % PPV is just the accuracy, too.
    ROC.Gaussian_model.PPV = ROC.Gaussian_model.sensitivity ./ (ROC.Gaussian_model.sensitivity + 1 - ROC.Gaussian_model.specificity);

    ROC.Gaussian_model.AUC_numerical_integration = aucn;
    ROC.Gaussian_model.AUC = normcdf(d_a_model/sqrt(2)); % should just invert the accuracy equation. ...and yes, it does. same as accuracy.

else
    % single-interval
    % ------------------------------------------------------------
    %     N1 = length(input_values(binary_outcome)); done above
    %     N2 = length(input_values(~binary_outcome));

    meanpres = mean(input_values(binary_outcome));
    meanabs = mean(input_values(~binary_outcome));

    v1 = var(input_values(binary_outcome));
    v0 = var(input_values(~binary_outcome));

    pooledsd = sqrt((v1.*(n1-1) + v0.*(n0-1)) ./ (n1 + n0 - 2));

    d = (meanpres - meanabs) ./ pooledsd;

    zpres = meanpres ./ pooledsd;
    zabs = meanabs ./ pooledsd;

    if donormfit
        % integrate across PDFs for each of signal absent, signal present distributions
        x = [zabs-3:.1:zpres+3]; % relative to null dist
        tprn = 1 - normcdf(x, zpres, 1);
        fprn = 1 - normcdf(x, zabs, 1);
        hold on;
        if doplot
            han = [han plot(fprn, tprn, '-', 'Color', color, 'LineWidth', linewid)];
        end
    end

    aucn = calc_auc(fprn, tprn);

    ROC.Gaussian_model.type = 'Single-interval';
    ROC.Gaussian_model.names = {'sig. abs.' 'sig. pres.'};
    ROC.Gaussian_model.means = [meanabs meanpres];
    ROC.Gaussian_model.n = [n0 n1];
    ROC.Gaussian_model.sd = [sqrt(v0) sqrt(v1)];
    ROC.Gaussian_model.mean_diff = meanpres - meanabs;
    ROC.Gaussian_model.pooledsd = pooledsd;
    ROC.Gaussian_model.d_a = d;
    ROC.Gaussian_model.sensitivity = 1 - normcdf(class_thr, meanpres, sqrt(v1));
    ROC.Gaussian_model.specificity = normcdf(class_thr, meanabs, sqrt(v0));
    ROC.Gaussian_model.PPV = ROC.Gaussian_model.sensitivity ./ (ROC.Gaussian_model.sensitivity + 1 - ROC.Gaussian_model.specificity);

    ROC.Gaussian_model.AUC_numerical_integration = aucn;
    ROC.Gaussian_model.AUC = normcdf(d/sqrt(2));
    ROC.Gaussian_model.all_vals = struct('fprn', fprn, 'tprn', tprn);

end

if doplot
    ROC.line_handle = han;
end

% Boostrap, if asked for
% -------------------------------------------------------------------------
if doboot

    [ci, names] = roc_boot(input_values, binary_outcome, ROC.class_threshold, 0);

    ROC.sensitivity_ci = ci{1};
    ROC.specificity_ci = ci{2};
    ROC.PPV_ci = ci{3};

end

% fprintf('\nROC_PLOT Output: %s, %s\n', ROC.Gaussian_model.type, threshold_type);
%
% fprintf('  Nonparametric AUC:\t%3.2f\tParametric d_a:\t%3.2f\n', ROC.AUC, ROC.Gaussian_model.d_a);
%
% fprintf('  Threshold:\t%3.2f\tSens:\t%3.0f%%\tSpec:\t%3.0f%%\tPPV:\t%3.0f%%\n', ...
%     ROC.class_threshold, 100*ROC.sensitivity, 100*ROC.specificity, 100*ROC.PPV);
%
% fprintf('  Accuracy:\t%3.0f%% +- %3.1f%% (SE), P = %3.6f\n', ...
%     100*ROC.accuracy, 100*ROC.accuracy_se, ROC.accuracy_p);

if doOutput
    % Single line format
    fprintf('\nROC_PLOT Output: %s, %s\n', ROC.Gaussian_model.type, threshold_type);

    if doboot
        fprintf('Threshold:\t%3.2f\tSens:\t%3.0f%% CI(%.0f%%-%.0f%%)\tSpec:\t%3.0f%% CI(%.0f%%-%.0f%%)\tPPV:\t%3.0f%% CI(%.0f%%-%.0f%%)\t', ...
            ROC.class_threshold, 100*ROC.sensitivity, 100*ROC.sensitivity_ci, 100*ROC.specificity, 100*ROC.specificity_ci, 100*ROC.PPV, 100*ROC.PPV_ci);
    else
        fprintf('Threshold:\t%3.2f\tSens:\t%3.0f%%\tSpec:\t%3.0f%%\tPPV:\t%3.0f%%\t', ...
            ROC.class_threshold, 100*ROC.sensitivity, 100*ROC.specificity, 100*ROC.PPV);
    end

    fprintf('Nonparametric AUC:\t%3.2f\tParametric d_a:\t%3.2f\t', ROC.AUC, ROC.Gaussian_model.d_a);

    fprintf('  Accuracy:\t%3.0f%% +- %3.1f%% (SE), P = %3.6f\n', ...
        100*ROC.accuracy, 100*ROC.accuracy_se, ROC.accuracy_p);
end

% Report stats (max sens) at 90% specificity
if reportstats90

    ROC = report_at_threshold_spec();

end

% Calculate statistics by criterion threshold value
% Parallels output of RSCOREplus by Lew Harvey
% -------------------------------------------------------------------------

bins = [-Inf plotthr Inf];   % bin edges: EDGES(k) <= X(i) < EDGES(k+1)
npoints = length(bins);
s0 = histc(input_values(~binary_outcome), bins)';
s1 = histc(input_values(binary_outcome), bins)';

bin_names = {};

for i = 1:length(bins)
    %bin_names{i} = sprintf('R_%3.1f', bins(i));
    bin_names{i} = sprintf('R_%d', i);
end

ROC.Binned_output.s0 = s0;
ROC.Binned_output.s1 = s1;
ROC.Binned_output.bin_edges = bins;
ROC.Binned_output.bin_names = bin_names;

ROC.Binned_output.spec_bins = cumsum(s0) ./ sum(s0);
ROC.Binned_output.sens_bins = 1 - (cumsum(s1) ./ sum(s1));
ROC.Binned_output.PPV_bins = (ROC.Binned_output.sens_bins .* sum(s1)) ./ (ROC.Binned_output.sens_bins .* sum(s1) + (1 - ROC.Binned_output.spec_bins) .* sum(s0));

if writerscoreplus

    write_rscoreplus_input()

end

% INLINE FUNCTIONS
% -------------------------------------------------------------------------

    function inline_plothistograms()

        if plothistograms

            if ~isempty(findobj(get(0, 'Children'), 'type', 'figure'))
                returncurrfig = 1;
            end

            if returncurrfig
                figh = gcf;
            end

            % plot histograms...

            create_figure('distributions');
            h = histfit(input_values(binary_outcome), 20);
            hbar = get(h(1), 'Children');
            set(hbar, 'FaceAlpha', .3, 'FaceColor', [0 0 1], 'EdgeColor', 'none');
            % changing in diff versions of matlab...
            if isempty(hbar)
                set(h(1), 'FaceAlpha', .3, 'FaceColor', [0 0 1], 'EdgeColor', 'none');
            end

            set(h(2), 'Color', [0 0 1]);

            hold on;
            h = histfit(input_values(~binary_outcome), 20);
            hbar = get(h(1), 'Children');
            set(hbar, 'FaceAlpha', .3, 'FaceColor', [0 1 0], 'EdgeColor', 'none');

            % changing in diff versions of matlab...
            if isempty(hbar)
                set(h(1), 'FaceAlpha', .3, 'FaceColor', [0 1 0], 'EdgeColor', 'none');
            end

            set(h(2), 'Color', [0 .4 0]);

            h = plot_vertical_line(class_thr);
            set(h, 'Color', 'k', 'LineStyle', '-', 'LineWidth', 2)

            if returncurrfig
                figure(figh)
            end

        end

    end % inline

    function write_rscoreplus_input()

        fname = 'roc_rscoreplus_input.txt';
        disp('Writing RScorePlus input .txt file: %s\n', fname);

        fid = fopen(fullfile(pwd, fname), 'w+');
        if fid == -1, disp('Cannot open rscoreplus_input.txt file. Skipping.'); return, end

        fprintf(fid, 'Heading\nroc_plot.m_output\n');

        % parameters => see rscore plus help on lew harvey's website
        fprintf(fid, '%01d\t%01d\t%01d\t%01d\t%01d\t%01d\t', length(bins), 2, 1, 0, 0, 1);

        if istwochoice, paradigmstr = 'MAFC'; else paradigmstr = 'SINT'; end
        fprintf(fid, '%s\n', paradigmstr);

        fprintf(fid, 'labels\t');
        fprintf(fid, '%s\t', bin_names{:});
        fprintf(fid, '\n');

        fprintf(fid, 's0\t');
        fprintf(fid, '%d\t', s0);
        fprintf(fid, '\n');

        fprintf(fid, 's1\t');
        fprintf(fid, '%d\t', s1);
        fprintf(fid, '\n');

        fprintf(fid, '%3.1f\t%3.1f\t%3.1f\t%3.1f\n', 0, 1, .5, 1); % params of reference dist?
        fprintf(fid, '%01d\t%01d\t%01d\t%01d\n', 0, 0, 1, 1); % params of reference dist?

        fprintf(fid, 'end of file\n-1\n');
        fclose(fid);

    end % inline

    function report_at_threshold_spec
        wh = find(fpr <= .1);
        [mymax, wh2] = max(tpr(wh));
        wh = wh(wh2);
        if ~isempty(wh), wh = wh(1); else wh = NaN; end

        npos = sum(tpr(wh) .* binary_outcome);
        nfp = sum(fpr(wh) .* ~binary_outcome);
        ppv = npos ./ (npos + nfp);

        if isnan(wh)
            fprintf('At 90+%% Spec: Thresh = %3.2f, Sensitivity = %3.0f%%, Specificity = %3.0f%%, PPV = %3.0f%%\n', NaN, NaN, NaN, NaN);

            [ROC.thresh_90_percent_spec, ROC.sens_90_percent_spec] = deal(NaN);
        else

            fprintf('At 90+%% Spec: Thresh = %3.2f, Sensitivity = %3.0f%%, Specificity = %3.0f%%, PPV = %3.0f%%\n', thr(wh), tpr(wh)*100, (1-fpr(wh))*100, ppv * 100);

            ROC.thresh_90_percent_spec = thr(wh);
            ROC.sens_90_percent_spec = tpr(wh);
        end

        wh = find(fpr <= .05);
        [mymax, wh2] = max(tpr(wh));
        wh = wh(wh2);
        if ~isempty(wh), wh = wh(1); else wh = NaN; end

        if isnan(wh)
            fprintf('At 95+%% Spec: Thresh = %3.2f, Sensitivity = %3.0f%%, Specificity = %3.0f%%, PPV = %3.0f%%\n', NaN, NaN, NaN, NaN);

            [ROC.thresh_95_percent_spec, ROC.sens_95_percent_spec] = deal(NaN);

        else
            fprintf('At 95+%% Spec: Thresh = %3.2f, Sensitivity = %3.0f%%, Specificity = %3.0f%%, PPV = %3.0f%%\n', thr(wh), tpr(wh)*100, (1-fpr(wh))*100, ppv * 100);

            ROC.thresh_95_percent_spec = thr(wh);
            ROC.sens_95_percent_spec = tpr(wh);
        end

    end % inline

end % main function

%
% function [xvals, tpr, fpr] = roc_calc(input_vals, binary_outcome, xvals)
% % Calculate Receiver Operating Characteristic plot (ROC) given P-values
% %
% %  function [xvals, tpr, fpr] = roc_calc(input_vals or input values, input_vals, [treshold vals to assess])
% %
% % Modified from roc_calc.m in scnlab tools
% % Tor Wager, 2012
%
%
% [tpr, fpr] = deal(zeros(size(xvals)));
%
% indx = 1;
% for x = xvals
%     wh = input_vals >= x;
%
%     tpr(indx) = sum(wh(binary_outcome)) ./ sum(binary_outcome);
%     fpr(indx) = sum(wh(~binary_outcome)) ./ sum(~binary_outcome);
%
%     indx = indx + 1;
% end
%
% end % function

function auc = calc_auc(fpr, tpr)

[u, wh] = unique(fpr);
u2 = tpr(wh);

% fix for AUC = 1 if no overlap; triangle method not perfectly accurate
% here.
if any(u == 0 & u2 == 1), auc = 1; return, end

for i = 2:length(u)

    xdiff = u(i) - u(i - 1);
    ydiff = u2(i) - u2(i - 1);
    a(i) = xdiff * u2(i - 1) + xdiff * ydiff / 2;  % area of rect + area of triangle

end

auc = sum(a);

end


% GetValueOfAssignment Gets the value of a variable assignment in a factor.
%
%   v = GetValueOfAssignment(F, A) returns the value of a variable assignment,
%   A, in factor F. The order of the variables in A are assumed to be the
%   same as the order in F.var.
%
%   v = GetValueOfAssignment(F, A, VO) gets the value of a variable assignment,
%   A, in factor F. The order of the variables in A are given by the vector VO.
%
%   See also SetValueOfAssignment.m and SampleFactors.m
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function v = GetValueOfAssignment(F, A, VO)

if (nargin == 2),
    indx = AssignmentToIndex(A, F.card);
else
    map = zeros(length(F.var), 1);
    for i = 1:length(F.var),
        map(i) = find(VO == F.var(i));
    end;
    indx = AssignmentToIndex(A(map), F.card);
end;

v = F.val(indx);

end


function V = hogDraw( H, w, fhog )
% Create visualization of hog descriptor.
%
% USAGE
%  V = hogDraw( H, [w], [fhog] )
%
% INPUTS
%  H          - [m n oBin*4] computed hog features
%  w          - [15] width for each glyph
%  fhog       - [0] if true draw features returned by fhog
%
% OUTPUTS
%  V          - [m*w n*w] visualization of hog features
%
% EXAMPLE
%
% See also hog, fhog
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.23
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% fold normalizations
if(nargin<3 || isempty(fhog)), fhog=0; end
m=size(H,3); if(fhog), m=(m-5)/3; H=H(:,:,1:m*3); m=3; else m=4; end
s=size(H); s(3)=s(3)/m; w0=H; H=zeros(s);
for o=0:m-1, H=H+w0(:,:,(1:s(3))+o*s(3)); end;

% construct a "glyph" for each orientaion
if(nargin<2 || isempty(w)), w=15; end
bar=zeros(w,w); bar(:,round(.45*w):round(.55*w))=1;
bars=zeros([size(bar) s(3)]);
for o=1:s(3), bars(:,:,o)=imrotate(bar,-(o-1)*180/s(3),'crop'); end

% make pictures of positive weights by adding up weighted glyphs
H(H<0)=0; V=zeros(w*s(1:2));
for r=1:s(1), rs=(1:w)+(r-1)*w;
  for c=1:s(2), cs=(1:w)+(c-1)*w;
    for o=1:s(3), V(rs,cs)=V(rs,cs)+bars(:,:,o)*H(r,c,o); end
  end
end


function [ia,ja,sa] = find(a)
%FIND         Implements  find(a)  for Taylor
%
%   index = find(a)
%   [ia,ja] = find(a)
%   [ia,ja,sa] = find(a)
%
% functionality as Matlab function find
%

% written  05/21/09     S.M. Rump
%

  if nargout<=1
    ia = find(any(a.t));
  elseif nargout==2
    index = reshape(1:prod(a.size),a.size);
    index(~any(a.t)) = 0;
    [ia,ja] = find(index);
  else
    ia = find(any(a.t));
    %VVVV  sa = a(ia);
    s.type = '()'; s.subs = {ia}; sa = subsref(a,s);
    %AAAA  Matlab bug fix
    index = reshape(1:prod(a.size),a.size);
    index(~any(a.t)) = 0;
    [ia,ja] = find(index);
  end


% DEMCMU35VARGPLVMRECONSTRUCTTAYLOR Reconstruct right leg and body of CMU 35.
%
% DESC
%  Load a model learned with vargplvm and reconstruct the missing indices
%  using the Taylor framework developed for fgplvm (angle errors)
% Based on demCmu35fgplvmReconstructTaylor.m

% See also: vargplvmTaylorAngleErrors
% VARGPLVM

randn('seed', 1e5);
rand('seed', 1e5);

% dynUsed=1;
%
% % Get the sequence numbers.
% [Y, lbls] = lvmLoadData('cmu35WalkJog'); %%%
% seq = cumsum(sum(lbls)) - [1:31]; %%%

dataSetName = 'cmu35gplvm';

if ~exist('experimentNo') experimentNo = 1; end

% load data
[Y, lbls, Ytest, lblstest] = lvmLoadData(dataSetName);

dataSetName = 'cmu35gplvm';

capName = dataSetName;
capName(1) = upper(capName(1));

fileName=['dem' capName 'Vargplvm' num2str(experimentNo)];

% load data
[Y, lbls, Ytest, lblstest] = lvmLoadData(dataSetName);

%---- Remove some sequences %%%%%%
%{
seqFrom=7;
seqEnd=7;

if seqFrom ~= 1
    Yfrom = seq(seqFrom-1)+1;
else
    Yfrom = 1;
end
Yend=seq(seqEnd);
Y=Y(Yfrom:Yend,:);
seq=seq(seqFrom:seqEnd);
seq=seq - ones(1,length(seq)).*(Yfrom-1);
%}
%-----

origBias = mean(Y);
origScale = 1./sqrt(var(Y));
% %scale = ones(size(scale));
% Y = Y - repmat(origBias, size(Y, 1), 1);
% Ytest = Ytest - repmat(origBias, size(Ytest, 1), 1);
% Y = Y.*repmat(origScale, size(Y, 1), 1);
% Ytest = Ytest.*repmat(origScale, size(Ytest, 1), 1);

% REMOVE LEG TEST
% Indices associated with right leg.
legInd = [8:14];
% Indices associated with upper body.
bodyInd = [21:50];

dt=0.05;
timeStampsTest = ([0:size(Ytest,1)-1].*dt)';

%for experimentNo = 1:3;

% Load saved model.
%load(['dem' capName num2str(experimentNo) '.mat']);
load(fileName); % From demCmu35gplvmVargplvm1.m
fprintf(1,'# Loaded %s\n',fileName);
if isfield(model, 'dynamics') & ~isempty(model.dynamics)
    fprintf(1,'# Dynamics kernel:');
    kernDisplay(model.dynamics.kern);
    model.dynamics.t_star = timeStampsTest;
    kernName = model.dynamics.kern.comp{1}.type;
else
    kernName = 'static';
end
fprintf(1,'# experimentNo=%d\n',experimentNo);
fprintf(1,'# Reconstruction Iters=%d\n',model.reconstrIters);

 model.reconstrIters = 2000; %%%TEMP

startInd = 63;

legErrs = vargplvmTaylorAngleErrors(model, Y, Ytest, startInd, origBias, origScale, legInd, [dataSetName ...
    'Leg'], experimentNo);

save(['dem' capName 'ReconstructLegs' num2str(experimentNo) '.mat'], 'legErrs');

%----
figPath = ['Results/CMU/' num2str(experimentNo) kernName '/Leg/'];
%try
%    saveAllOpenFigures(figPath, [],1)
%catch
%    %
%end
%----

legErrs

bodyErrs = vargplvmTaylorAngleErrors(model, Y, Ytest, startInd, origBias, origScale, bodyInd, [dataSetName ...
    'Body'], experimentNo);

save(['dem' capName 'ReconstructBody' num2str(experimentNo) '.mat'], 'bodyErrs');

figPath = ['Results/CMU/' num2str(experimentNo) kernName '/Body/'];
%try
%    saveAllOpenFigures(figPath, [],1)
%catch
%    %
%end

%----
bodyErrs

% bar(model.kern.comp{1}.inputScales)

%end


function [vacc, hacc, vcm, hcm] = mcmcProcessResult_pixels(imsegs, pg)
% same as mcmcProcessResult except that pg is a pixel confidence map

vcm = zeros(3);
hcm = zeros(5);
vtotal = 0;
htotal = 0;
vacc = 0;
hacc = 0;
for f = 1:numel(pg)

    [imh, imw, nc] = size(pg{f});
    pg{f} = reshape(pg{f}, [imh*imw nc]);

    pgv = [pg{f}(:, 1) sum(pg{f}(:, 2:6), 2) pg{f}(:, 7)];
    pgh = pg{f}(:, 2:6);

    vlab = imsegs(f).vert_labels(imsegs(f).segimage(:));
    hlab = imsegs(f).horz_labels(imsegs(f).segimage(:));
    npix = numel(vlab);
    vlab = vlab(:);
    hlab = hlab(:);

    [maxval, vmax] = max(pgv, [], 2);
    [maxval, hmax] = max(pgh, [], 2);

    vacc = vacc + sum(vlab==vmax)/npix;
    vtotal = vtotal + sum(vlab>0)/npix;

    hacc = hacc + sum((hlab==hmax) & (vlab==2))/npix;
    htotal = htotal + sum((hlab>0) & (vlab==2))/npix;

    for s = 1:numel(vmax)
        if vlab(s)~=0
            vcm(vlab(s), vmax(s)) = vcm(vlab(s), vmax(s)) + 1/npix;
        end
        if hlab(s)~=0
            hcm(hlab(s), hmax(s)) = hcm(hlab(s), hmax(s)) + 1/npix;
        end
    end
end

vacc = vacc / vtotal;
hacc = hacc / max(htotal, 1E-9);

vcm = vcm ./ max(repmat(sum(vcm, 2), 1, size(vcm, 2)), 1E-9);
hcm = hcm ./ max(repmat(sum(hcm, 2), 1, size(hcm, 2)), 1E-9);

%disp(['vacc: ' num2str(vacc) '   hacc: ' num2str(hacc)])
%disp(num2str(sum(ny, 1) / sum(totalv)))


function PlottinMaxWithPNorm

p = 64;

%nIteration = 498;
%fCase = 'ExperimentingPlot';
%folder = ['/home/alex/git-repos/Swan/Output/',fCase];

%nIteration = 545;
%fCase = 'LatticeExperimentInputCantileverSymmetricMeshSuperEllipsePDE';
%folder = '/media/alex/My Passport/LatticeResults/CantileverSymmetricMeshSuperEllipsePDEEpsilonEhGradient2000';

nIteration = 235;
fCase = 'LatticeExperimentInputCantileverSymmetricMeshSuperEllipsePDE';
folder = '/media/alex/My Passport/LatticeResults/CantileverSymmetricMeshSuperEllipsePDEGradientEpsilonH';

%nIteration = 2362;
%fCase = 'LatticeExperimentInputCantileverSymmetricMeshSuperEllipsePDE';
%folder = '/media/alex/My Passport/LatticeResults/CantileverSymmetricMeshSuperEllipsePDE10000iteration';
for iter = 1:nIteration

    s.fileName = [fCase,num2str(iter)];
    s.folderPath = fullfile(folder);

    wM = WrapperMshResFiles(s);
    wM.compute();

    mesh = wM.mesh;

    quad = Quadrature.set(mesh.type);
    quad.computeQuadrature('CONSTANT');

    dvolum = mesh.computeDvolume(quad);
    sl2Norm = wM.dataRes.StressNormGauss;
    sl2NormP = (sl2Norm).^(p);
    int = sl2NormP.*dvolum';
    intOpt = int(:);
    stressNorml2LpNorm = sum(intOpt);
    stresLpNorm(iter)  = stressNorml2LpNorm.^(1/p);
    stressMax(iter)    = max(abs(sl2Norm));
    iter

    if iter == 1 || iter/10 == floor(iter/10)
        print(folder,fCase,stresLpNorm,stressMax,iter)
    end

end
print(folder,fCase,stresLpNorm,stressMax,iter)
end

function print(folder,fCase,stresLpNorm,stressMax,nIteration)

fid = fopen(fullfile(folder,[fCase,'.txt']));
tLine = textscan(fid,'%s','delimiter','\n', 'headerlines',0);

a = tLine{1};
costA = a(end-9,1);
costAs = split(costA);
costAt = str2double(costAs(2));

a = tLine{1};
costR = a(end,1);
costRs = split(costR);
costRt = str2double(costRs(2));

const = costRt/costAt;

fNameMon = fullfile(folder,'Monitoring.fig');
h = openfig(fNameMon);
handles = findobj(h,'Type','line');
iterC = get(handles(5),'Xdata');
cost = get(handles(5),'Ydata');
close all
f = figure();
pN{1} = plot(iterC,cost*const);
hold on
pN{2} = plot(1:nIteration,stresLpNorm);
hold on
pN{3} = plot(1:nIteration,stressMax);

leg = legend({'Cost function','L^{64}(\Omega) norm','max norm'});
set(leg);

savefig(f,fullfile(folder,[fCase,'CostVsLinf.fig']))

p = plotPrinter(f,pN);
p.print(fullfile(folder,[fCase,'Cost']));

end


function test10 ( dim_num, n, z, ns, sample_routine, seed_init )

%*****************************************************************************80
%
%% TEST10 tests TAU_MEASURE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST10\n' );
  fprintf ( 1, '  TAU_MEASURE computes the TAU measure of quality.\n' );
  fprintf ( 1, '  2nd moment trace measure       Tau = %14f\n', ...
    tau_measure ( dim_num, n, z, ns, sample_routine, seed_init ) );

  return
end


function applanet_prt(jdtdb)

% print aphelion/perihelion conditions

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global au pnames drsaved iap_flg iplanet1

% convert julian day to calendar date and time strings

[cdstr, utstr] = jd2str(jdtdb);

if (iap_flg == 1)

    % perihelion

    fprintf('\ntime and conditions at perihelion of ');
    disp(deblank(pnames(iplanet1, 1:7)));
    fprintf('============================================\n\n');

else

    % aphelion

    fprintf('\ntime and conditions at aphelion of ');
    disp(deblank(pnames(iplanet1, 1:7)));
    fprintf('==========================================\n\n');

end

fprintf('calendar date                ');

disp(cdstr);

fprintf('\nTDB time                     ');

disp(utstr);

fprintf('\nTDB Julian date              %16.8f \n', jdtdb);

jdutc = tdb2utc (jdtdb);

[cdstr, utstr] = jd2str(jdutc);

fprintf('\nUTC time                     ');

disp(utstr);

fprintf('\nUTC Julian date              %16.8f \n\n', jdutc);

% display heliocentric distance (kilometers and AU)

fprintf('heliocentric distance        %16.6f kilometers\n', drsaved);

fprintf('                             %16.12f AU', drsaved / au);

% toggle minima flag

iap_flg = -iap_flg;


function r8vec_index_delete_one_test ( )

%*****************************************************************************80
%
%% R8VEC_INDEX_DELETE_ONE_TEST tests R8VEC_INDEX_DELETE_ONE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n_max = 25;
  n = 0;
  x = [];
  indx = [];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_INDEX_DELETE_ONE_TEST\n' );
  fprintf ( 1, '  R8VEC_INDEX_DELETE_ONE deletes one copy of a\n' );
  fprintf ( 1, '  particular value.\n' );
  fprintf ( 1, '\n' );

  xval = 8.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  xval = 7.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  seed = 123456789;

  for i = 1 : 20
    [ xval, seed ] = r8_uniform_ab ( 0.0, 20.0, seed );
    xval = round ( xval );
    fprintf ( 1, '  %f\n', xval );
    [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );
  end

  xval = 7.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  xval = 8.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indexed list of entries:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I  INDX(I)  X(I)  X(INDX(I))\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %3d  %3d  %9f  %9f\n', i, indx(i), x(i), x(indx(i)) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Call R8VEC_INDEX_DELETE_ONE to delete one value of 8:\n' );

  xval = 8.0;
  [ n, x, indx ] = r8vec_index_delete_one ( n, x, indx, xval );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indexed list of entries:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I  INDX(I)  X(I)  X(INDX(I))\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %3d  %3d  %9f  %9f\n', i, indx(i), x(i), x(indx(i)) );
  end

  return
end


%% Example
% Using iiwa as a plotter, the user can mount:
% 1- A pen, in such a case the robot is going to draw
% 2- A laser, in such a case the robot is going to cut the object

% This script utilizes the real time control of the KST
% where the iiwa7R800 is specified, if you have iiwa14R820 uncomment the
% code-line:
%

% Note that for acheiving a precise drawing do the following steps:
% A- On the Sunrise Code of the KST project:
%         1- Open the file (MatlabToolboxServer.java)
%         2- Locate the function:
%             double getTheDisplacment(double dj)
%             {
%                 double   a=0.07;
%                 double b=a*0.75;
%                 double exponenet=-Math.pow(dj/b, 2);
%                 return Math.signum(dj)*a*(1-Math.exp(exponenet));
%
%             }
%         3- Change it into
%             double getTheDisplacment(double dj)
%             {
%                 return dj;
%             }
% B- Check the pdf file in the folder (Tips and tricks) of the github repo,
% and reconfigure your PC according to the recomendations of the file.
% C- Use a PC with good specifications. The PC used for running this script
% is Corei7-6850k processor @3.6GHZ with  32 G RAM.

% After performing the previous steps, synchronise the Sunrise project of
% the KST into the controller.
% Then start the MatlabToolboxServer application on the KUKA iiwa controller
% Then run this script using Matlab

% Copyright: Mohammad SAFEEA, 26th of April 2018

close all,clear all;clc;
%% Add KST to Matlab path
kst_Path=getTheKSTDirectory(pwd);
addpath(kst_Path);

warning('off')

%% Create the robot object
ip='172.31.1.147'; % The IP of the controller
arg1=KST.LBR7R800; % choose the robot iiwa7R800
% arg1=KST.LBR14R820; % un comment this line if you have iiwa14R820
arg2=KST.Medien_Flansch_Touch_pneumatisch; % choose the type of flange
Tef_flange=eye(4); % transofrm matrix of EEF with respect to flange
Tef_flange(3,4)=0.293; % length of the tep of the pen from the surface of the flange
iiwa=KST(ip,arg1,arg2,Tef_flange); % create the object

%% Start a connection with the server
flag=iiwa.net_establishConnection();
if flag==0
  disp('Error unable to connected to the controller');
  return;
else
  disp('Successfully connected to the controller');
end
pause(0.5);

%% Go to an initial configuration
jPos={0,0,0,-pi/2,0,pi/2,0};
relVel=0.15;
iiwa.movePTPJointSpace(jPos, relVel); % move to initial configuration
%% Get Cartesian position of EEF (homing position)
fprintf('Current cartesian position:')
eefpos=iiwa.getEEFPos();
X=eefpos{1};
Y=eefpos{2};
Z=eefpos{3};
eefposDist=eefpos;
disp(eefpos);
%% Motion parameters
deltaZ=5; % Small clearance from the plotting-surface (mm)
z_sheet= 81.2; % the Z coordinates of the plotting-surface (mm)
zUp=z_sheet+deltaZ;
vFast=25/1000; % fast motion velocity, m/sec
vSlow=20/1000; % slow motion velocity, m/sec
%% Read the Cad file
fileName='kst.plt'; % this is the name of the cad file,
% in case you have the cad file in another folder the user can use the
% total path of the file, the file shall be in plt format
[plotFlag,corArray]=loadPltFileFun(fileName); % load the file
%% Move robot down to a posiiton close to the sheet
% fast motion
eefposDist{1}=corArray(1,1)+X;
eefposDist{2}=corArray(2,1)+Y;
eefposDist{3}=zUp;
iiwa.movePTPLineEEF(eefposDist, 100);
%% Start direct servo in Joint space
% get joints angles of robot
jPos  = iiwa.getJointsPos();
% calculate current position of flange point of the robot
qs=zeros(7,1);
for i=1:7
    qs(i)=jPos{i};
end

TefTool=eye(4);
[T0,~]=iiwa.directKinematics(qs); % Transformation matrix at the tip of the pin (TCP)
x=T0(1,4);
y=T0(2,4);

zUp1=T0(3,4);
zDown1=T0(3,4)-deltaZ/1000;

Tt=T0;
n=max(size(corArray));
%% scale and displace the drawing in the plane
scaleX=5;
scaleY=10;
dispY=-1.7;
dispX=-0.22;
corArray(1,:)=corArray(1,:)*scaleX+dispX;
corArray(2,:)=corArray(2,:)*scaleY+dispY;

for i=1:n
    corArray(1,i)=corArray(1,i)+x;
    corArray(2,i)=corArray(2,i)+y;
end
% start the direct servo
 iiwa.realTime_startDirectServoJoints();
disp('Starting direct servo joint space');
%% Start control loop
tic;
vz=0.003;
for i=1:n-1
    if(plotFlag(i)==0)
        [Tt,qs]=goDown(iiwa,qs,TefTool,vz,zDown1);
        v=vSlow;
    else
        [Tt,qs]=goUp(iiwa,qs,TefTool,vz,zUp1);
        v=vFast;
    end
    x0=x;
    x1=corArray(1,i);
    y0=y;
    y1=corArray(2,i);
    wattar=((x0-x1)*(x0-x1)+(y0-y1)*(y0-y1))^0.5;
    if wattar==0
        continue;
    end
    vx=v*(x1-x0)/wattar;
    vy=v*(y1-y0)/wattar;
    trajectoryTime0=toc;
    transmittionTime0=trajectoryTime0;
    motionFlag=1;
    while motionFlag
        deltaT=toc-trajectoryTime0;
        x=x0+vx*deltaT;
        y=y0+vy*deltaT;
        %
        wattar1=((x0-x)*(x0-x)+(y0-y)*(y0-y))^0.5;
        if wattar1>wattar
            motionFlag=0;
        end
        if(toc-transmittionTime0)>0.003
            Tt(1,4)=x;
            Tt(2,4)=y;
            [ qs ] = iiwa.gen_InverseKinematics( qs, Tt, 10,0.1 );
            transmittionTime0=toc;
            for k=1:7
                jPos{k}=qs(k);
            end
            iiwa.sendJointsPositionsf(jPos);
        end

    end
end
[Tt,qs]=goUp(iiwa,qs,TefTool,vz,zUp1);
%% Turn off direct servo control
iiwa.realTime_stopDirectServoJoints();
%% Move robot back to home position
% fast motion
eefposDist{1}=X;
eefposDist{2}=Y;
eefposDist{3}=Z;
iiwa.movePTPLineEEF(eefposDist, 100);

iiwa.net_turnOffServer();


function r = DH_bccr(HazeImg,conf)

%  Gaofeng MENG:
%  National Laboratory of Pattern Recognition,
%  Institute of Automation, Academy of Sciences, Beijing 100190
%  Comments and bug reports are welcome.  Email to gfmeng@nlpr.ia.ac.cn
%
%  WORK SETTING:
%  This code has been compiled and tested by using MATLAB R2009a
%
%  For more detials, please see our paper:
%  Gaofeng MENG, Ying WANG, Jiangyong DUAN, Shiming XIANG, Chunhong PAN.
%  Efficient Image Dehazing with Boundary Constraint and Contextual Regularization,
%  ICCV, Sydney, Australia, pp.617-624, 3-6 Dec., 2013.
%
%  Last Modified: Feb. 14, 2014, By Gaofeng MENG

%% param
bccr_method = conf.bccr_method;
bccr_air_wsz = conf.bccr_air_wsz; % airlight window size
bccr_Bou_wsz = conf.bccr_Bou_wsz; % Boundcon window size
bccr_lambda = conf.bccr_lambda; % regularization parameter, the more this parameter, the closer to the original patch-wise transmission

%%
A = bccr_Airlight(HazeImg, bccr_method, bccr_air_wsz);

% calculating boundary constraints
ts = bccr_Boundcon(HazeImg, A, 30, 300, bccr_Bou_wsz);

% refining the estimation of transmission
t = bccr_CalTransmission(HazeImg, ts, bccr_lambda, 0.5); % using contextual information

% dehazing
r = bccr_Dehazefun(HazeImg, t, A, 0.85);


function analyse_dev_lin_and_qual_fusion(key,scores_obj_array,scr_lin,scr_qual,prior)
% Analyses the dev scores for all input systems (including fusions)
% to so that there relative strength can be compared.  It
% calculates the percentage of target trials calculated by each
% system and the actual dcf, min dcf and prbep values for each
% system.
% Inputs:
%   key: The Key for the dev trials.
%   scores_obj_array: An array of score objects (one object for
%     each system) that was fused.
%   scr_lin: An object of type Scores containing the result of the
%     linear fusion.
%   scr_qual: An object of type Scores containing the result of the
%     quality fusion.
%   prior: The effective target prior.

assert(nargin==5)
assert(isa(key,'Key'))
assert(isa(scr_lin,'Scores'))
assert(isa(scr_qual,'Scores'))
assert(key.validate())
assert(scr_lin.validate())
assert(scr_qual.validate())

trialmask = key.to_ndx().trialmask;
dev_stacked_scores = stackScores(key,scores_obj_array);
dev_lin_fusion = scr_lin.scoremat(trialmask(:))';
dev_qfusion = scr_qual.scoremat(trialmask(:))';
dev_scores = [dev_stacked_scores;dev_lin_fusion;dev_qfusion];

true_prop = sum(key.tar(:))/(sum(key.tar(:))+sum(key.non(:)));
logprint(Logger.Info,'True Dev target proportion is %g%%. System estimates are:\n',100*true_prop);
logprint(Logger.Info,'%s',sprintfmatrix(100*estimate_target_proportions(dev_scores,true_prop)'));

numsystems = size(dev_stacked_scores,1);
mf = size(dev_scores,1);
logprint(Logger.Info,'\n\nAnalysing Dev: %i subsystems and %i fusions:\n',numsystems,mf-numsystems);
E = evalAll(dev_scores,key,prior);
logprint(Logger.Info,'\nDev all: dcf*100, mindcf*100, prbep:\n%s',sprintfmatrix(E,1:mf));
end


function [v,x] = max(S1F,varargin)

x = linspace(0,2*pi);
y = S1F.eval(x);
[v,ind] = max(y);

x = x(ind);


function BC = createBC3D(meshvar)
% Creates a boundary condition structure from a mesh structure
% for a 3D structured mesh. The default boundary conditions on
% all boundaries are Neumann;
% The values of each boundary condition are defined as:
% BC.<position>. :
%               a, b, c, where
% a*grad(phi).e + b*phi = c
%
% SYNOPSIS:
%   BC = createBC3D(meshvar)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Extract number of cells from the mesh structure
Nxyz = meshvar.dims;
Nx = Nxyz(1); Ny = Nxyz(2); Nz = Nxyz(3);

% Define the top, bottom, right, and left boundary conditions
% (default = Neumann, i.e., a = 1, b = 0, c = 0)
top.a = ones(Nx,Nz);
top.b = zeros(Nx,Nz);
top.c = zeros(Nx,Nz);
top.periodic = 0;

bottom.a = ones(Nx,Nz);
bottom.b = zeros(Nx,Nz);
bottom.c = zeros(Nx,Nz);
bottom.periodic = 0;

right.a = ones(Ny,Nz);
right.b = zeros(Ny,Nz);
right.c = zeros(Ny,Nz);
right.periodic = 0;

left.a = ones(Ny,Nz);
left.b = zeros(Ny,Nz);
left.c = zeros(Ny,Nz);
left.periodic = 0;

front.a = ones(Nx,Ny);
front.b = zeros(Nx,Ny);
front.c = zeros(Nx,Ny);
front.periodic = 0;

back.a = ones(Nx,Ny);
back.b = zeros(Nx,Ny);
back.c = zeros(Nx,Ny);
back.periodic = 0;

BC= BoundaryCondition(meshvar, left, right, bottom, top, back, front);


function [C, S] = QLiftDec2Nevill(X, N, filtername)
%-----------------------------------------------------------------------------
% QLiftDec2Nevill
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% Calls for: NevilleR2Q, stencilR2Q, stencilCrop, stencilxgridfRVC, QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2NV
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: May 16, 2002.
% (c) 1999-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2Nevill - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2Nevill - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), filtername) < N
    error(' QLiftDec2Nevill - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2Nevill - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
C = []; S = [];
%
%Secondly, some initializing of filters
%
switch lower(filtername)
 case 'neville2'
   [Pa, centerPa] = NevilleR2Q(2);
 case 'neville4'
   [Pa, centerPa] = NevilleR2Q(4);
 case 'neville6'
   [Pa, centerPa] = NevilleR2Q(6);
 case 'neville8'
   [Pa, centerPa] = NevilleR2Q(8);
 otherwise
   error([' QLiftDec2Nevill - unknown filter ' filtername]);
end
%
% Here Pa is the stencil of a prediction step,
% e.g. Pa = 0.250*[0 1 0; 1 0 1; 0 1 0]; centerPa = [2 2];
%
Ua = 0.5 * Pa; centerUa = centerPa;
% Ua is the stencil of the update step and as such determined by Pa.
%
[Pb, centerPb] = stencilR2Q(Pa, centerPa);
[Pb, centerPb] = stencilCrop(Pb, centerPb);
% where Pb is the stencil of a prediction step,
% e.g. Pb = 0.250*[1 0 1; 0 0 0; 1 0 1]; centerPb = [2 2];
%
Ub = 0.5 * Pb; centerUb = centerPb; % Compare to V11!!,
%                                     what about center of stencil??
% Ub is the stencil of the update step and as such determined by Pb.
%
%Thirdly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2Nevill - too many levels ');
   else
     sizeO = size(O);
   end
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
%  Convolution of prediction stencil Pa with WHOLE gridfunction O.
%  Note: the operations count is twice as much as it could be.
   PaO = stencilxgridfRVC(O, Pa, centerPa);
%
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - getcolor01(PaO);
   Q1001D10 = getcolor10(O) - getcolor10(PaO);
   clear PaO;
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%  Note: this is not an "in place" implementation.
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   RectDetail = putcolor10(Q1001D10, sizeO) + putcolor01(Q1001D01, sizeO);
   clear Q1001D01 Q1001D10;
   UaD = stencilxgridfRVC(RectDetail, Ua, centerUa);
   clear RectDetail;
%
   Q0011A00 = getcolor00(O) + getcolor00(UaD);
   Q0011A11 = getcolor11(O) + getcolor11(UaD);
   clear UaD O;
%
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - ...
      getcolor11(stencilxgridfRVC(putcolor00(Q0011A00, sizeO), Pb, centerPb));
   clear Q0011A11;
%  Note: the operations count is twice as much as it could be
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
%
%  Stage: update
%  Break up next line in parts in case checking is desired
   APPROX00 = Q0011A00 + ...
      getcolor00(stencilxgridfRVC(putcolor11(DETAIL11, sizeO), Ub, centerUb));
   clear Q0011A00 DETAIL11;
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function c = prod(a,dim)
%PROD         Implements  prod(a,dim)  for gradients
%
%   c = prod(a,dim)
%
% functionality as Matlab function prod for matrices, parameter dim optional
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  [m n] = size(a);
  if nargin==1,
    if m==1
      dim=2;
    else
      dim=1;
    end
  end

  if dim==1
    c = ones(1,n);
    for i=1:m
%VVVV c = c .* a(i,:);
      s.type = '()'; s.subs = {i,':'}; c = c .* subsref(a,s);
%AAAA Matlab V5.2 bug fix
    end
  else
    c = ones(m,1);
    for i=1:n
%VVVV c = c .* a(:,i);
      s.type = '()'; s.subs = {':',i}; c = c .* subsref(a,s);
%AAAA Matlab V5.2 bug fix
    end
  end

  if rndold
    setround(rndold)
  end


function [F_xw,F_sprocedure] = filter_sprocedure(F_xw,w,uncertaintyModel,ops)

% Select the constraints to analyse
if any(is(F_xw,'elementwise'))
    F_lp = F_xw(find(is(F_xw,'elementwise')));
    % and save the rest for later analysis
    F_xw = F_xw(find(~is(F_xw,'elementwise')));

    p = sdpvar(F_lp);
    keep = ones(1,length(p));
    F_sprocedure = [];

    % We cannot use a duality based SOS-decomposition
    opsin = ops;
    ops.verbose = 0;
    ops.sos.model=2;

    % All decision variables (i.e. not variables in SOS)
    Parameters = [recover(setdiff(depends(p),depends(w)))];
    for i = 1:length(p)
        d = degree(p(i),w);
        if all(d<=2) & any(d==2)
            if opsin.verbose
                disp(' - Using exact S-procedure to eliminate uncertainty');
            end
            lambda = sdpvar(1);
            if isempty(uncertaintyModel{1}.r)
                g = uncertaintyModel{1}.g;
            else
                e = (w-uncertaintyModel{1}.center);
                g = uncertaintyModel{1}.r^2-e'*e;
            end
            Parameters = [Parameters;lambda];
            F_sprocedure = [F_sprocedure, sos(p(i)-lambda*g), lambda>=0];
            %F_sprocedure = [F_sprocedure, lambda>0, compilesos(sos(p(i)-lambda*g),[],ops,s)];
            keep(i) = 0;
        elseif 0%any(d>2)
            [multiplier, lambda] = polynomial(w,2);
            e = (w-uncertaintyModel{1}.center);
            g = uncertaintyModel{1}.r^2-e'*e;
            Parameters = [Parameters;lambda];
            F_sprocedure = [F_sprocedure,lambda>=0,sos(p(i)-multiplier*g), sos(multiplier)];
            %F_sprocedure = [F_sprocedure, lambda>0, compilesos(sos(p(i)-multiplier*g)+sos(multiplier),[],ops,s)];
            keep(i) = 0;
        end
    end
    if ~isempty(F_sprocedure)
        F_sprocedure = compilesos(F_sprocedure,[],ops,Parameters);
    end
    if any(keep)
        F_xw = [F_xw,p(find(keep))>=0];
    end
else
    F_sprocedure = [];
end


% AUTHOR:         Aaron Nicolson
% AFFILIATION:    Signal Processing Laboratory, Griffith University
%
% This Source Code Form is subject to the terms of the Mozilla Public
% License, v. 2.0. If a copy of the MPL was not distributed with this
% file, You can obtain one at http://mozilla.org/MPL/2.0/.

clear all; close all; clc;

%% GET MATLAB_FEAT REPOSITORY
addpath('~/Dropbox/GitHub/matlab_feat/feat')
addpath('./deepxi')

%% PARAMETERS
T_d = 32; % window duration (ms).
T_s = 16; % window shift (ms).
f_s = 16000; % sampling frequency (Hz).
s.N_d = round(f_s*T_d*0.001); % window duration (samples).
s.N_s = round(f_s*T_s*0.001); % window shift (samples).
s.f_s = f_s; % sampling frequency (Hz).
s.NFFT = 2^nextpow2(s.N_d); % frequency bins (samples).
d = s; x = s;
SNR_avg = -5:5:15; % SNR levels used to compute average SD level.

%% DIRECTORIES
s.dir = '/home/aaron/set/deep_xi_test_set/test_clean_speech';
d.dir = '/home/aaron/set/deep_xi_test_set/test_noise';
gamma_hat_dir = input('gamma_hat path:', 's');
gamma_hat_dir_split = strsplit(gamma_hat_dir, '/');
ver = [gamma_hat_dir_split{end-2}, '_', gamma_hat_dir_split{end-1}];

%% FILE LISTS
gamma_hat_paths = dir([gamma_hat_dir, '/*.mat']); % noise file paths.

results = MapNested();
noise_src_set = {};
SNR_set = {};
for i = 1:length(gamma_hat_paths)

    load([gamma_hat_paths(i).folder, '/', gamma_hat_paths(i).name])

    if any(isnan(gamma_hat(:))) || any(isinf(gamma_hat(:)))
        error('NaN or Inf value in gamma_hat: %s.', gamma_hat_paths(i).name)
    end

    split_basename = strsplit(gamma_hat_paths(i).name,'_');
    noise_src = split_basename{end-1};
    SNR = split_basename{end};
    clean_speech = extractBefore(gamma_hat_paths(i).name, ['_', noise_src, '_', SNR]);
    SNR = SNR(1:end-6);

    s.wav = audioread([s.dir, '/', clean_speech, '_', noise_src, '.wav']); % clean speech.
    d.src = audioread([d.dir, '/', clean_speech, '_', noise_src, '.wav']); % noise.
    [x.wav, d.wav] = add_noise(s.wav, d.src(1:length(s.wav)), str2double(SNR)); % noisy speech.

    x = analysis_stft(x, 'polar'); % noisy-speech STMS.
    d = analysis_stft(d, 'polar'); % noise STMS.

    gamma = (x.STMS.^2)./(d.STMS.^2); % instantaneous a posteriori SNR.

    gamma_hat = gamma_hat(1:size(gamma, 1), :);
    D = spectral_distortion(gamma, gamma_hat);

    if any(isnan(D(:))) || any(isinf(D(:)))
        error('NaN or Inf value in D.')
    end

    if ~any(strcmp(SNR_set, SNR))
        SNR_set{end+1} = SNR;
    end

    if ~any(strcmp(noise_src_set, noise_src))
        noise_src_set{end+1} = noise_src;
    end

    if results.isKey(noise_src, SNR)
        results(noise_src, SNR) = [D; results(noise_src, SNR)];
    else
        results(noise_src, SNR) = D;
    end
    clc;
    fprintf('%.2f%%\n', 100*i/length(gamma_hat_paths));
end

% there has to be a better way to do this.
for i=1:length(SNR_set)
    SNR_set{i} = str2num(SNR_set{i});
end
tmp = sort(cell2mat(SNR_set));
for i=1:length(SNR_set)
    SNR_set{i} = num2str(tmp(i));
end

res_dir = 'log/results/spectral_distortion_gamma';
if ~exist(res_dir, 'dir')
    mkdir(res_dir)
end

fileID = fopen([res_dir, '/', ver, '.csv'],'w');
fprintf(fileID, 'noise, snr_db, SD\n');
avg = [];
for i=1:length(noise_src_set)
    for j=1:length(SNR_set)
        D = mean(results(noise_src_set{i}, SNR_set{j}));
        fprintf('%s, %s: %.2f.\n', noise_src_set{i}, SNR_set{j}, D);
        fprintf(fileID, '%s, %s, %.2f\n', noise_src_set{i}, SNR_set{j}, D);
        if ismember(j, SNR_avg)
            avg = [avg; results(noise_src_set{i}, SNR_set{j})];
        end
    end
end
fclose(fileID);

avg_path = [res_dir, '/average.csv'];
if ~exist(avg_path, 'file')
    fileID = fopen(avg_path, 'w');
    fprintf(fileID, 'ver, SD\n');
    fclose(fileID);
end
fileID = fopen(avg_path, 'a');
fprintf(fileID, '%s, %.2f\n', ver, mean(avg));
fclose(fileID);


function [fusedBox] = fusePETMRI(ROIbox_PET,ROIbox_MRI,maskBox_PET,MRIinv,MRIweight,wavelet)
% -------------------------------------------------------------------------
% function [fusedBox] = fusePETMRI(ROIbox_PET,ROIbox_MRI,maskBox_PET,Invert,MRIweight,wavelet)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function fuses the region of interest (ROI) of two registered PET and
% MRI volumes using a technique based on the wavelet transform. See Ref. [1]
% for more details.
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% -------------------------------------------------------------------------
% INPUTS:
% - ROIbox_PET: 3D array of the smallest box containing the ROI of the PET
%               volume.
% - ROIbox_MRI: 3D array of the smallest box containing the ROI of the MRI
%               volume.
% - maskBox_PET: 3D array of the mask of the smallest box specifying the
%                ROI of the PET volume. Voxels within the ROI are assigned
%                value of 1, voxels outside a value of 0.
% - MRIinv: String specifying if the intensities of the MRI volume are
%           inverted prior to fusion with PET. Either 'Inv' for inversion,
%           or 'NoInv' for no inversion.
% - MRIweight: Numerical value specifying the weight of the MRI scan in the
%              fusion with PET.
% - wavelet: String specifying the name of the MATLAB wavelet basis used
%            wavelet basis used in the fusion process.
% -------------------------------------------------------------------------
% OUTPUTS:
% - fusedBox: 3D array of the smallest box containing the ROI of the fused
%             PET/MRI volume. Note that the output contains random values
%             outside the ROI. These values are added in the fusion process
%             in order to accurately calculate the mean and the standard
%             deviation of the ROI (only) of the MRI volume when performing
%             Collewet normalization in the wavelet domain. Apply the mask
%             to 'fusedBox' by setting NaNs outside the ROI to accurately
%             visualize the fusion.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% PET PRE_PROCESSING
ROIbox_PET = sqrt(ROIbox_PET);

% RESAMPLING MRI VOLUME TO PET IN-PLANE RESOLUTION (slice spacing
% previously verified to be the same)
szPET = size(ROIbox_PET);
temp=zeros(szPET);
for i = 1:szPET(3)
    temp(:,:,i) = imresize(ROIbox_MRI(:,:,i),[szPET(1),szPET(2)],'Method','cubic','Antialiasing',true);
end
ROIbox_MRI = temp;

% NORMALIZATION (and inversion) OF VOLUMES
ROIOnly_MRI = ROIbox_MRI;
ROIOnly_PET = ROIbox_PET;
ROIOnly_MRI(maskBox_PET==0) = NaN;
ROIboxFill_MR = fillBox(ROIOnly_MRI);
ROIOnly_PET(maskBox_PET==0) = NaN;
minMRI = min(ROIOnly_MRI(:));
ROIboxFill_MR = ROIboxFill_MR - minMRI;
ROIOnly_MRI = ROIOnly_MRI - minMRI;
ROIboxFill_MR = ROIboxFill_MR./max(ROIOnly_MRI(:)).*255;
if strcmp(MRIinv,'Inv')
    ROIboxFill_MR = 255 - ROIboxFill_MR;
end
minPET = min(ROIOnly_PET(:));
ROIbox_PET = ROIbox_PET - minPET;
ROIOnly_PET = ROIOnly_PET - minPET;
ROIbox_PET = ROIbox_PET./max(ROIOnly_PET(:)).*255;

% WAVELET DECOMPOSITION AND FUSION OF COEFFICIENTS
wdecMRI = wavedec3(ROIboxFill_MR,1,wavelet);
wdecPET = wavedec3(ROIbox_PET,1,wavelet);
wdecFUSED = wdecPET;
nbcell = length(wdecMRI.dec);
for i = 1:nbcell
    wdecMRI.dec{i} = CollewetNorm(wdecMRI.dec{i});
    wdecMRI.dec{i}(isnan(wdecMRI.dec{i})) = wdecPET.dec{i}(isnan(wdecMRI.dec{i}));
    wdecFUSED.dec{i} = (1-MRIweight).*wdecPET.dec{i} + MRIweight.*wdecMRI.dec{i};
end

% WAVELET RECONSTRUCTION
fusedBox = waverec3(wdecFUSED);
fusedBox = fusedBox - min(fusedBox(:));
fusedBox = fusedBox./max(fusedBox(:)).*255;

end


classdef prtClassPerTurbo < prtClass
    % See: "Perturbo: a new classi?cation algorithm based on the spectrum
    % perturbations of the laplace-beltrami operator."
    %
    % Notes:
    % 1) Very very sensitive to the kernel sigma parameter; should/must
    % optimize over \sigma
    %
    % 2) Very memory intensive.  Consider bootstrapping data and/or bagging
    % this classifier to reduce computational load (memory ~ O(nObs^2) )
    %
    % % try this:
    % ds = prtDataGenMarysSimpleSixClass;
    % pt = prtClassPerTurbo;
    % pt.kernel = prtKernelRbf('sigma',.7);
    % pt = pt.train(ds);
    % plot(pt)
    %
    %
    % ds = prtDataGenMarySimple;
    % pt = prtClassPerTurbo;
    % pt.kernel = prtKernelRbf('sigma',.7);
    % pt = pt.train(ds);
    % plot(pt)
    %

    properties (SetAccess=private)
        name = 'PerTurbo'  % Bumping
        nameAbbreviation = 'PerTurbo'  %
        isNativeMary = true;         %
    end

    properties
        kernel = prtKernelRbfNdimensionScale;
    end
    properties (SetAccess=protected)
        trainedKernels
        K
        Kinv
        uClasses
    end

    methods

        function self = set.kernel(self,val)
            assert(numel(val)==1 &&  isa(val,'prtKernel'),'prt:prtClassRvm:kernel','kernel must be a prtKernel');
            self.kernel = val;
        end

        function self = prtClassPerTurbo(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)
            self.uClasses = dataSet.uniqueClasses;
            y = dataSet.getTargets;
            x = dataSet.getObservations;
            for i = 1:length(self.uClasses)
                cInd = y == self.uClasses(i);
                cx = x(cInd,:);

                tempDs = prtDataSetClass(cx);
                self.trainedKernels{i} = self.kernel.train(tempDs);
                self.K{i} =  self.trainedKernels{i}.run_OutputDoubleArray(tempDs);
                self.Kinv{i} = inv(self.K{i});
            end
        end

        function yOut = runAction(self,dataSet)

            x = nan(dataSet.nObservations,length(self.uClasses));
            for samples = 1:1000:dataSet.nObservations
                retainObs = samples:min([samples+999,dataSet.nObservations]);
                currSet = dataSet.retainObservations(retainObs);
                for i = 1:length(self.uClasses)

                    k = self.trainedKernels{i}.run_OutputDoubleArray(currSet);
                    x(retainObs,i) = 1-diag(k*self.Kinv{i}*k');
                end
            end

            yOut = dataSet;
            yOut.X = -x;
            %binary; note - output mixture for binary classification
            if length(self.uClasses) == 2
                yOut.X = -x(:,2)+x(:,1);
            end
        end

    end
end


function r = subsref(a,s)
%SUBSREF      Implements subscripted references for Hessians
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/03/12     S.M. Rump  INTLAB_HESSIAN_DERIV_ERROR removed
% modified 10/06/12     S.M. Rump  internal use
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');

  while 1
    if ~isa(a,'hessian')                 % index reference a.x(i) etc.
      r = subsref(a,s(1));
    elseif strcmp(s(1).type,'()')        % index reference a(i)
      r.x = a.x(s(1).subs{:});
      index = reshape(1:prod(size(a.x)),size(a.x));
      index = index(s(1).subs{:});
      r.dx = a.dx(:,index(:));
      r.hx = a.hx(:,index(:));
      % avoid Matlab 6.5f bug:
      % a = sparse([],[],[],1,1); a = reshape(a,1,1); abs(a)
      % produces  9.6721e-317  or similar number in underflow range
      if prod(size(r.x))==1
        r.x = full(r.x);
      end
      if prod(size(r.dx))==1
        r.dx = full(r.dx);
      end
      if prod(size(r.hx))==1
        r.hx = full(r.hx);
      end
      r = class(r,'hessian');
    elseif strcmp(s(1).type,'.')         % index reference a.x, a.dx or a.hx
      if strcmp(s(1).subs,'x')
        r = a.x;
      elseif strcmp(s(1).subs,'dx')
        sizeax = size(a.x);
        if ( length(sizeax)==2 ) & ( sizeax(2)==1 )
          sizeax = sizeax(1);            % row gradient for column vector a.x
        end
        if issparse(a.dx) & ( length(sizeax)>1 )
          error('access of .dx sparse hessian with more than one column, see hessianinit')
        end
        r = reshape(transpose(a.dx),[sizeax N]);
      elseif strcmp(s(1).subs,'ddx')
        r = a.dx;
      elseif strcmp(s(1).subs,'hx')
        if issparse(a.hx) & ( prod(size(a.x))>1 )
          error('access of .hx of non-scalar sparse hessian, see hessianinit')
        end
        index = reshape(1:N*N,N,N)';
        r = a.hx + a.hx(index(:),:);     % Hessian is .hx + transpose(.hx)
        sizeax = size(a.x);
        if prod(sizeax)==1
          r = reshape(r,N,N);
        else
          r = reshape(r,[sizeax N N]);
        end
      elseif strcmp(s(1).subs,'hhx')
        index = reshape(1:N*N,N,N)';
        r = a.hx + a.hx(index(:),:);     % Hessian is .hx + transpose(.hx)
      elseif strcmp(s(1).subs,'mid')
        r = mid(a);
      else
        error('invalid subscript reference for hessian')
      end
    else
      error('invalid index reference for hessian')
    end
    if length(s)==1
      if rndold
        setround(rndold)
      end
      return
    end
    s = s(2:end);
    a = r;
  end


function [traj, infStates] = tapas_ehgf(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_ehgf(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_ehgf(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_ehgf_transp(r, p);
end

% Number of levels
l = length(p)/5;

if l ~= floor(l)
    error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
end

% Unpack parameters
mu_0 = p(1:l);
sa_0 = p(l+1:2*l);
rho  = p(2*l+1:3*l);
ka   = p(3*l+1:4*l-1);
om   = p(4*l:5*l-2);
th   = exp(p(5*l-1));
al   = 1/p(5*l);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];

% Number of trials (including prior)
n = length(u);

% Assume that if u has more than one column, the last contains t
try
    if r.c_prc.irregular_intervals
        if size(u,2) > 1
            t = [0; r.u(:,end)];
        else
            error('tapas:hgf:InputSingleColumn', 'Input matrix must contain more than one column if irregular_intervals is set to true.');
        end
    else
        t = ones(n,1);
    end
catch
    if size(u,2) > 1
        t = [0; r.u(:,end)];
    else
        t = ones(n,1);
    end
end

% Initialize updated quantities

% Representations
mu = NaN(n,l);
pi = NaN(n,l);

% Other quantities
muhat = NaN(n,l);
pihat = NaN(n,l);
v     = NaN(n,l);
w     = NaN(n,l-1);
da    = NaN(n,l);
dau   = NaN(n,1);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu(1,:) = mu_0;
pi(1,:) = 1./sa_0;

% Representation update loop
% Pass through trials
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 1st level
        % ~~~~~~~~~
        % Prediction
        muhat(k,1) = mu(k-1,1) +t(k) *rho(1);

        % Precision of prediction
        pihat(k,1) = 1/(1/pi(k-1,1) +t(k) *exp(ka(1) *mu(k-1,2) +om(1)));

        % Input prediction error
        dau(k) = u(k) -muhat(k,1);

        % Updates
        pi(k,1) = pihat(k,1) +1/al;
        mu(k,1) = muhat(k,1) +1/pihat(k,1) *1/(1/pihat(k,1) +al) *dau(k);

        % Volatility prediction error
        da(k,1) = (1/pi(k,1) +(mu(k,1) -muhat(k,1))^2) *pihat(k,1) -1;

        if l > 2
            % Pass through higher levels
            % ~~~~~~~~~~~~~~~~~~~~~~~~~~
            for j = 2:l-1
                % Prediction
                muhat(k,j) = mu(k-1,j) +t(k) *rho(j);

                % Precision of prediction
                pihat(k,j) = 1/(1/pi(k-1,j) +t(k) *exp(ka(j) *mu(k-1,j+1) +om(j)));

                % Weighting factor
                v(k,j-1) = t(k) *exp(ka(j-1) *mu(k-1,j) +om(j-1));
                w(k,j-1) = v(k,j-1) *pihat(k,j-1);

                % Mean update
                mu(k,j) = muhat(k,j) +1/2 *1/pihat(k,j) *ka(j-1) *w(k,j-1) *da(k,j-1);

                % Ingredients of precision update which depend on the mean
                % update
                vv = t(k) *exp(ka(j-1) *mu(k,j) +om(j-1));
                pimhat = 1/(1/pi(k-1,j-1) +vv);
                ww = vv *pimhat;
                rr = (vv -1/pi(k-1,j-1)) *pimhat;
                dd = (1/pi(k,j-1) +(mu(k,j-1) -muhat(k,j-1))^2) *pimhat -1;

                % Precision update
                pi(k,j) = pihat(k,j) +max(0, 1/2 *ka(j-1)^2 *ww*(ww +rr*dd));

                % Volatility prediction error
                da(k,j) = (1/pi(k,j) +(mu(k,j) -muhat(k,j))^2) *pihat(k,j) -1;
            end
        end

        % Last level
        % ~~~~~~~~~~
        % Prediction
        muhat(k,l) = mu(k-1,l) +t(k) *rho(l);

        % Precision of prediction
        pihat(k,l) = 1/(1/pi(k-1,l) +t(k) *th);

        % Weighting factor
        v(k,l)   = t(k) *th;
        v(k,l-1) = t(k) *exp(ka(l-1) *mu(k-1,l) +om(l-1));
        w(k,l-1) = v(k,l-1) *pihat(k,l-1);

        % Mean update
        mu(k,l) = muhat(k,l) +1/2 *1/pihat(k,l) *ka(l-1) *w(k,l-1) *da(k,l-1);

        % Ingredients of the precision update which depend on the mean
        % update
        vv = t(k) *exp(ka(l-1) *mu(k,l) +om(l-1));
        pimhat = 1/(1/pi(k-1,l-1) +vv);
        ww = vv *pimhat;
        rr = (vv -1/pi(k-1,l-1)) *pimhat;
        dd = (1/pi(k,l-1) +(mu(k,l-1) -muhat(k,l-1))^2) *pimhat -1;

        pi(k,l) = pihat(k,l) +max(0, 1/2 *ka(l-1)^2 *ww*(ww +rr*dd));

        % Volatility prediction error
        da(k,l) = (1/pi(k,l) +(mu(k,l) -muhat(k,l))^2) *pihat(k,l) -1;
    else

        mu(k,:) = mu(k-1,:);
        pi(k,:) = pi(k-1,:);

        muhat(k,:) = muhat(k-1,:);
        pihat(k,:) = pihat(k-1,:);

        v(k,:)  = v(k-1,:);
        w(k,:)  = w(k-1,:);
        da(k,:) = da(k-1,:);

    end
end

% Remove representation priors
mu(1,:)  = [];
pi(1,:)  = [];

% Remove other dummy initial values
muhat(1,:) = [];
pihat(1,:) = [];
v(1,:)     = [];
w(1,:)     = [];
da(1,:)    = [];
dau(1)     = [];

% Create result data structure
traj = struct;

traj.mu     = mu;
traj.sa     = 1./pi;

traj.muhat  = muhat;
traj.sahat  = 1./pihat;

traj.v      = v;
traj.w      = w;
traj.da     = da;
traj.dau    = dau;

% Updates with respect to prediction
traj.ud = mu -muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,l);
psi(:,1)   = 1./(al*pi(:,1));
psi(:,2:l) = pihat(:,1:l-1)./pi(:,2:l);
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,l);
epsi(:,1)   = psi(:,1) .*dau;
epsi(:,2:l) = psi(:,2:l) .*da(:,1:l-1);
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,l);
wt(:,1)   = psi(:,1);
wt(:,2:l) = 1/2 *(v(:,1:l-1) *diag(ka(1:l-1))) .*psi(:,2:l);
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,l,4);
infStates(:,:,1) = traj.muhat;
infStates(:,:,2) = traj.sahat;
infStates(:,:,3) = traj.mu;
infStates(:,:,4) = traj.sa;

end


 function Asums = osem_sum(G, yi, ci, ri, block_order)
%	precompute some sums needed for OSEM
%	NOT DONE!!!!!!!!!!!
%	block_order is a vector of length nblock
%	that specifies the starting angle for each block
%	simplest case is just block_order = 1:nblock,
%	but that choice is suboptimal
%	if block_order is an empty matrix, then 1 block is used
%	if block_order is a scalar (nblock) power of 2 != 1,
%	then the "bit-reversal ordering" is used
%	(nblock can be 1 to na)
%
%	Copyright Apr 1999, Jeff Fessler

if nargin < 3, help osem_sum, error args, end

[nb, na] = size(yi);

if (nargin < 4 || isempty(ci))
	ci = ones(size(yi));
end
if (nargin < 5 || isempty(ri))
	ri = zeros(size(yi));
end
if (nargin < 6 || isempty(block_order))
	block_order = 1;
end
nblock = length(block_order);
if length(block_order) == 1
	nblock = block_order;
	block_order = 1:nblock;
end
if (nargin < 7 || isempty(Asums))
	% compute Asum

	not done!!!!!!!1
	for ii=1:nblock
		ib = block_order(ii);
	end
	Asum = zeros([size(x) nblock]);
end
if (nargin < 7 || isempty(nblock))
	nblock = 1;
end

Gt = G';	% easier to work with transpose

for ib=1:nblock
	ia = ib:nblock:na;
	Asum = zeros(size(x));
	eterm = zeros(size(x));

	%	Asum = G' * ci
	for ia=ib:nblock:na
		Asum = Asum + Gt(:,[1:nb]+(ia-1)*na) * ci(ia,:)';
	end
	return
	yp = ci .* (G * x) + ri;	% predicted measurements
	eterm = G' * (ci .* (yi ./ yp));
	x = x .* eterm ./ Asum;
end


% SP_DRCHLT_L2_PROJ: assign the degrees of freedom of Dirichlet boundaries through an L2 projection.
%
%   [u, dofs] = sp_drchlt_l2_proj (sp, msh, h, sides)
%
% INPUT:
%
%  sp:    object defining the space of discrete functions (see sp_scalar)
%  msh:   object defining the domain partition and the quadrature rule (see msh_cartesian)
%  h:     function handle to compute the Dirichlet condition
%  sides: boundary sides on which a Dirichlet condition is imposed
%
% OUTPUT:
%
%  u:    assigned value to the degrees of freedom
%  dofs: global numbering of the corresponding basis functions
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2010, 2011, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function [u, dofs] = sp_drchlt_l2_proj (sp, msh, h, sides)

  rhs  = zeros (sp.ndof, 1);

  % In the 1D case, with an open knot vector,  it is not necessary to compute a projection.
  % For now it only works for scalars
  if (msh.ndim == 1)
    dofs = []; u = zeros (numel(sides), 1);
    for ii = 1:numel(sides)
      iside = sides(ii);
      dofs = union (dofs, sp.boundary(iside).dofs);
      if (iside == 1)
        geo_map = msh.map(msh.breaks{1}(1));
      else
        geo_map = msh.map(msh.breaks{1}(end));
      end
      for jj = 1:msh.rdim; xx{jj} = geo_map(jj); end
      u(ii) = h(xx{:}, iside);
    end
    u = u(:);
    return
  end

  dofs = [];
  nent = 0;
  for iside = sides
    nent = nent + msh.boundary(iside).nel * sp.boundary(iside).nsh_max^2;
    dofs = union (dofs, sp.boundary(iside).dofs);
  end

  rows = zeros (nent, 1);
  cols = zeros (nent, 1);
  vals = zeros (nent, 1);

  ncounter = 0;
  for iside = sides
% Restrict the function handle to the specified side, in any dimension, hside = @(x,y) h(x,y,iside)
    hside = @(varargin) h(varargin{:},iside);
    [rs, cs, vs] = op_u_v_tp (sp.boundary(iside), sp.boundary(iside), msh.boundary(iside));

    bnd_dofs = sp.boundary(iside).dofs;

    rows(ncounter+(1:numel(rs))) = bnd_dofs(rs);
    cols(ncounter+(1:numel(rs))) = bnd_dofs(cs);
    vals(ncounter+(1:numel(rs))) = vs;
    ncounter = ncounter + numel (rs);

    rhs(bnd_dofs) = rhs(bnd_dofs) + op_f_v_tp (sp.boundary(iside),msh.boundary(iside), hside);
  end

  M = sparse (rows(1:ncounter), cols(1:ncounter), vals(1:ncounter));
  u = M(dofs, dofs) \ rhs(dofs, 1);

end


filename='Cantilever_tetrahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'IPOPT';
incrementFactor = 1;
designVariable = 'Density';
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;


function [beadsAt,bestslice]=ExtractCoordinates(myres,s)
%Error: Undefined function or method 'ExtractCoordiantes' for input arguments of type 'dip_image'

[mymax,maxpos]=max(myres,[],3);
mymax
fprintf('Please select bead coordinates by left clicking. Finish with right click\n');
beadsAt=dipgetcoords(100);
fprintf('%d beads selected\n',size(beadsAt,1)-1);
beadsAt(end,:)=[];

for b=1:size(beadsAt,1)
    mypos=beadsAt(b,:);
    mybead=extract(squeeze(mymax),[s s],mypos);

    mymask=mybead >= max(mybead);
    mybeadslice=extract(squeeze(maxpos),[s s],mypos);
    bestslice(b)= round(mean(mybeadslice,mymask));

%    Version 1
%     [mm,mpx]=max(mybead,[],1); mpx=squeeze(mpx);
%     [mm,mppx]=max(squeeze(mm),[],1);
%     bestslice(b,1) = double(mpx(mppx));
%     [mm,mpy]=max(mybead,[],2);mpy=squeeze(mpy);
%     [mm,mppy]=max(squeeze(mm),[],1);
%     bestslice(b,2) = double(mpy(mppy));

end


function f = toFunctionOut(disc, coeffs, cutoff)
%TOFUNCTIONOUT   Convert discrete values of an ULTRAS discretization to a
%   CHEBFUN.
%
%   F = TOFUNCTIONOUT(DISC, COEFFS, CUTOFF) converts the coeffs returned by
%   ULTRAS to a CHEBFUN. The input may be piecewise smooth, as indicated by the
%   dimension property of the discretization.

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

% Check for cutoff
if ( nargin == 3 )
    m = cutoff;
else
    m = inf;
end

dom = disc.domain;         % Domain we're working on, including breakpoints
c = mat2cell(full(coeffs), disc.dimension); % Break into smooth pieces
funs = cell(numel(c), 1);  % One FUN per piece

% adjust size of cutoff if necessary
if ( length(m) ~= numel(c) )
    m = max(m)*ones(numel(c), 1);
end

for k = 1:numel(c)
    % Construct CHEBTECH2 objects from each piece:
    coeffs = c{k};
    c{k} = coeffs(1:min(m(k), size(coeffs, 1)), :);
    ct = chebtech2({[], c{k}});
    % Assign each piece to a subinterval with a BNDFUN:
    funs{k} = bndfun(ct, struct('domain', dom(k: k + 1)));
end
% Conver the FUNS cell-array to a CHEBFUN.
f = chebfun(funs);

end


function square_hex_grid_test05 ( )

%*****************************************************************************80
%
%% SQUARE_HEX_GRID_TEST05 tests HEX_GRID_01_APPROXIMATE_N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 4

  n_goal_test = [ 100, 200, 500, 10000 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_HEX_GRID_TEST05\n' );
  fprintf ( 1, '  For a hexagonal grid of points in the unit box,\n' );
  fprintf ( 1, '  HEX_GRID_01_APPROXIMATE_N seeks the value of\n' );
  fprintf ( 1, '  NODES_PER_LAYER that produces a mesh of about N points.\n' );
  fprintf ( 1, '\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  N_GOAL  NODES_PER_LAYER       N\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    n_goal = n_goal_test ( test );

    [ nodes_per_layer, n ] = hex_grid_01_approximate_n ( n_goal );

    fprintf ( 1, '  %6d           %6d  %6d\n', n_goal, nodes_per_layer, n );

  end

  return
end


function myerode(mask_file,new_mask_file)
% This code is written by Liang Zhan (zhan.liang@ucla.edu)
% Input variables
% 1) make_file name (nifti format, no gz format)
% 2) new_mask_file name (this is output in nifti format)

nii=load_untouch_nii(mask_file);
mask=double(nii.img);
dim1=size(mask,1); dim2=size(mask,2); dim3=size(mask,3);
for zs=1:dim1
   mask(zs,:,:)=bwmorph(squeeze(mask(zs,:,:)),'erode');
end
for zs=1:dim2
   mask(:,zs,:)=bwmorph(squeeze(mask(:,zs,:)),'erode');
end
for zs=1:dim3
mask(:,:,zs)=bwmorph(mask(:,:,zs),'erode');
end
nii.img=mask;
save_untouch_nii(nii,new_mask_file);
end


classdef layerTotalAvgPool

    properties
        type= 'custom'
        name= 'avgTotal'
        precious= false
    end

    methods

        function l= layerTotalAvgPool(name)
            if nargin>0, l.name= name; end
        end

    end

    methods (Static)

        function res1= forward(p, res0, res1)
            res1.x= vl_nnpool(res0.x, [size(res0.x,1), size(res0.x,2)], ...
                'method', 'avg');
        end

        function res0= backward(p, res0, res1)
            res0.dzdx= vl_nnpool(res0.x, [size(res0.x,1), size(res0.x,2)], res1.dzdx, ...
                'method', 'avg');
        end

    end

end


function Q_gppca = mohsst5_experiment_vbtdsepinit(data, D, maxiter, densityW, ...
                                             densityX, updatepseudow, ...
                                             updatepseudox)

if nargin < 3
  maxiter = 10;
end
if nargin < 6
  updatepseudow = true;
end
if nargin < 7
  updatepseudox = true;
end

[M,N] = size(data.observations);

% Divide data into test and train sets
testsize = 20; % testset size in percents
Itest = load(sprintf('/share/bayes/data/jaakko/mohsst5/ind%dtest.mat',testsize));
Itrain = load(sprintf('/share/bayes/data/jaakko/mohsst5/ind%dtrain.mat',testsize));
Ytest = data.observations;
Ytest(Itrain.Itrain) = nan;
Yobs = data.observations;
Yobs(Itest.Itest) = nan;

% Run VB PCA
disp('-- Run VB PCA --')
Q_vbpca = pca_full(Yobs,D,'maxiters',min(15,maxiter),'rotate2pca',true, ...
                   'algorithm','vb');

%tsplot(Q_vbpca.S)

% Run TDSEP
disp('-- Run TDSEP --')
sel = [0 1 2 3 5 7 10 15 20 25 30 40 50 70 100];
%sel = ceil(0:50);
%sel = ceil(exp(0:5));
R = tdsep2(Q_vbpca.S, sel);
R
[Q_vbpca.A, Q_vbpca.Av, Q_vbpca.S, Q_vbpca.Sv] = vbpca_rotate(R, Q_vbpca.A, ...
                                                  Q_vbpca.Av, Q_vbpca.S, ...
                                                  Q_vbpca.Sv);

%tsplot(Q_vbpca.S)

%return

% $$$ % Load VB PCA results with 80 components
% $$$ Q_vbpca = load('/home/alexilin/matlab/kaplan/vbresults80.mat');

disp('Initialize with VB PCA results');
Qinit.W = Q_vbpca.A(:,1:D);
Qinit.X = Q_vbpca.S(1:D,:);
Qinit.tau = 1/Q_vbpca.V;
Qinit.varW = zeros(size(Qinit.W));
Qinit.varX = zeros(size(Qinit.X));
for m=1:M
  v = diag(Q_vbpca.Av{m});
  Qinit.varW(m,:) = v(1:D);
end
for n=1:N
  v = diag(Q_vbpca.Sv{n});
  Qinit.varX(:,n) = v(1:D);
end
% Put the mean as the last component.. :|
Qinit.X(end,:) = 1;
Qinit.varX(end,:) = 0;
Qinit.W(:,end) = Q_vbpca.Mu;
Qinit.varW(:,end) = Q_vbpca.Muv;

% Covariance functions
% W distances in kilometers
switch 1
 case 1
  gpcovW = {@gpcovScale, @(logtheta,x1,x2) gpcov(logtheta,x1,x2,@sqdistEarth)};
  logthetaW = [log(2); log(1e3)];
  Wcov = 'se';
end
% X distances in days
switch 2
  case 2
   gpcovX = @gpcovPP;
   logthetaX = log(80);
   Xcov = 'cs';
end

for d=1:D
  covfunW{d} = gpcovW;
  covfunX{d} = gpcovX;
  initthetaW{d} = logthetaW;
  initthetaX{d} = logthetaX;
end

datestring = datestr(now, 'yyyymmdd');
filename = sprintf(['/share/bayes/data/jaakko/mohsst5/' ...
                    'mohsst5_vbtdsepinit_testsize=%d_D=%d_Wcov=%s_Wden=' ...
                    '%d_Xcov=%s_Xden=%d_iter=%d_date=%s'], testsize, D, ...
                   Wcov, ceil(100*densityW), Xcov, ceil(100*densityX), ...
                   maxiter, datestring)

disp('-- Run GP PCA --')
Q_gppca = vbgppcamv(Yobs, D,...
                    data.coordinates, data.time,...
                    covfunW, initthetaW,...
                    covfunX,initthetaX, ...
                    'init', Qinit, ...
                    'maxiter',maxiter, ...
                    'pseudodensityx', densityX, ...
                    'pseudodensityw', densityW, ...
                    'loglikelihood', true, ...
                    'updatehyper', [1 1 10:10:(maxiter-1)], ...
                    'updatepseudox', updatepseudox, ...
                    'updatepseudow', updatepseudow, ...
                    'maxsearchx', 3, ...
                    'maxsearchw', 3, ...
                    'checkgradx', false, ...
                    'autosavetime', 3600, ...
                    'autosavefile', filename);

Yh_gppca = Q_gppca.W * Q_gppca.X;
%Yh_vbpca = Q_vbpca.A * Q_vbpca.S + repmat(Q_vbpca.Mu,1,N);

trainrmse_gppca = rmse(Yobs, Yh_gppca)
%trainrmse_vbpca = rmse(Yobs, Yh_vbpca)
testrmse_gppca = rmse(Ytest, Yh_gppca)
%testrmse_vbpca = rmse(Ytest, Yh_vbpca)

% Huuuuuge matrices... Don't save them..
Q_gppca.CovXp = [];
Q_gppca.CovWp = [];

Q_gppca.filename = filename;

%save(filename, '-struct', 'Q_gppca');
save(filename, '-struct', 'Q_gppca');


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% Setup NIREP problem (data needs to be obtained
% separately)
%
% =========================================================================

dataset='na03';
example = ['3D-nirep-',dataset];
checkSetupDataFile; if OK, return; end;

% set view options and interpolation options
[viewer,viewOptn] = viewImage('reset','viewImage','imgmontage','colormap','gray(256)','direction','-zyx');

% setup interpolation scheme
imgOptn  = {'imgModel','splineInterMex','regularizer','moments','theta',.01};

% setup transformation used in the parametric part
traOptn  = {'trafo','affine3D'};

% setup distance measure
disOptn  = {'distance','SSD'};

% initialize the regularizer for the non-parametric part
regOptn = {'regularizer','mbHyperElastic','alpha',1,'alphaLength',1,'alphaArea',.1,'alphaVolume',2};

FAIRmessage(mfilename)
load('na01-128x150x128');
load('na01-128x150x128-labels');
load([dataset,'-128x150x128']);
load([dataset,'-128x150x128-labels']);
dataT = double(dataT);
dataR = double(dataR);
dataTl = double(dataTl);
dataRl = double(dataRl);
m = size(dataT);

dataT = 256.*dataT;
dataR = 256.*dataR;
max(dataT(:))

%omega = [0,m(1),0,m(2),0,m(3)];
omega = [0,20,0,23.4375,0,20];
ML = getMultilevel({dataT,dataR},omega,m,'fig',2);
save(outfile,'dataT','dataR','dataTl','dataRl','omega','m','ML');
save(outfile,'-append','viewOptn','imgOptn','traOptn','disOptn','regOptn');
checkSetupDataFile;

% xc       = getCellCenteredGrid(omega,m);
% viewData = @(I) viewImage(imgModel(I,omega,xc),omega,m);

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