function o = nifti2struc
% Create a data structure describing NIFTI-2 headers
%__________________________________________________________________________
% Copyright (C) 2005-2017 Wellcome Trust Centre for Neuroimaging

%
% $Id: nifti2struc.m 7147 2017-08-03 14:07:01Z spm $

persistent org;
if ~isempty(org)
    o = org;
    return;
end
t = struct(...
    'conv',{@char  , @uint8 , @int16 , @int32 , @int64 , @single , @double },...
    'prec',{'uint8', 'uint8', 'int16', 'int32', 'int64', 'single', 'double'},...
    'size',{   1   ,    1   ,    2   ,    4   ,    8   ,    4    ,    8    });
c = t(1);
b = t(2);
s = t(3);
i = t(4);
l = t(5);
f = t(6);
d = t(7);

table = {...
    i,  1, 'sizeof_hdr', 540
    c,  8, 'magic', ['ni2' char(0) sprintf('\r\n\032\n')]
    s,  1, 'datatype', 2
    s,  1, 'bitpix', 8
    l,  8, 'dim', [3 0 0 0 1 1 1 1]
    d,  1, 'intent_p1', 0
    d,  1, 'intent_p2', 0
    d,  1, 'intent_p3', 0
    d,  8, 'pixdim', [0 1 1 1]
    l,  1, 'vox_offset', 0
    d,  1, 'scl_slope', 1
    d,  1, 'scl_inter', 0
    d,  1, 'cal_max', []
    d,  1, 'cal_min', []
    d,  1, 'slice_duration', []
    d,  1, 'toffset', []
    l,  1, 'slice_start', []
    l,  1, 'slice_end', []
    c, 80, 'descrip', 'NIFTI-2 Image'
    c, 24, 'aux_file', ''
    i,  1, 'qform_code', 0
    i,  1, 'sform_code', 0
    d,  1, 'quatern_b', 0
    d,  1, 'quatern_c', 0
    d,  1, 'quatern_d', 0
    d,  1, 'qoffset_x', 0
    d,  1, 'qoffset_y', 0
    d,  1, 'qoffset_z', 0
    d,  4, 'srow_x', [1 0 0 0]
    d,  4, 'srow_y', [0 1 0 0]
    d,  4, 'srow_z', [0 0 1 0]
    i,  1, 'slice_code', []
    i,  1, 'xyzt_units', 10
    i,  1, 'intent_code', 0
    c, 16, 'intent_name', ''
    b,  1, 'dim_info', []
    c, 15, 'unused_str', ''};

org = struct('label',table(:,3),'dtype',table(:,1),'len',table(:,2),...
    'offset',0,'def',table(:,4));
os  = 0;
for j=1:length(org)
    os  = org(j).dtype.size*ceil(os/org(j).dtype.size);
    fun = org(j).dtype.conv;
    if ischar(org(j).def), z = char(0); else z = 0; end
    def = [org(j).def repmat(z,1,org(j).len-length(org(j).def))];
    org(j).def    = feval(fun,def);
    org(j).offset = os;
    os  = os + org(j).len*org(j).dtype.size;
end
o = org;


function x = jpeg2k2im(y)
%JPEG2K2IM Decodes an IM2JPEG2K compressed image.
%   X = JPEG2K2IM(Y) decodes compressed image Y, reconstructing an
%   approximation of the original image X.  Y is an encoding structure
%   returned by IM2JPEG2K.
%
%   See also IM2JPEG2K.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

narginchk(1,1);       % Check input arguments

% Get decoding parameters: scale, quantization vector, run-length
% table size, zero run code, end-of-data code, wavelet bookkeeping
% array, and run-length table.
n = double(y.n);
q = double(y.q) / 100;
runs = double(y.runs);
zrc = -double(y.zrc);
eoc = zrc - 1;
s = double(y.s);
s = reshape(s,n + 2,2);

% Compute the size of the wavelet transform.
cl = prod(s(1,:));
for i = 2:n + 1
   cl = cl + 3 * prod(s(i,:));
end

% Perform Huffman decoding followed by zero run decoding.
r = huff2mat(y.huffman);

c = [];   zi = find(r == zrc);    i = 1;
for j = 1:length(zi)
   c = [c r(i:zi(j) - 1) zeros(1,runs(r(zi(j) + 1)))];
   i = zi(j) + 2;
end

zi = find(r == eoc);                % Undo terminating zero run
if length(zi) == 1                  % or last non-zero run.
   c = [c r(i:zi - 1)];
   c = [c zeros(1,cl - length(c))];
else
   c = [c r(i:end)];
end

% Denormalize the coefficients.
c = c + (c > 0) - (c < 0);
for k = 1:n
   qi = 3 * k - 2;
   c = wavepaste('h',c,s,k,wavecopy('h',c,s,k) * q(qi));
   c = wavepaste('v',c,s,k,wavecopy('v',c,s,k) * q(qi + 1));
   c = wavepaste('d',c,s,k,wavecopy('d',c,s,k) * q(qi + 2));
end
c = wavepaste('a',c,s,k,wavecopy('a',c,s,k) * q(qi + 3));

% Compute the inverse wavelet transform and level shift.
x = waveback(c,s,'jpeg9.7',n);
x = uint8(x + 128);


function CPDpot = convert_dbn_CPDs_to_tables(bnet, evidence)
% CONVERT_DBN_CPDS_TO_TABLES Convert CPDs of (possibly instantiated) DBN nodes to tables
% CPDpot = convert_dbn_CPDs_to_tables(bnet, evidence)
%
% CPDpot{n,t} is a table containing P(n,t|pa(n,t), ev)
% All hidden nodes are assumed to be discrete.
% We assume the observed nodes are the same in every slice.
%
% Evaluating the conditional likelihood of long evidence sequences can be very slow,
% so we take pains to vectorize where possible.

[ss T] = size(evidence);
%obs_bitv = ~isemptycell(evidence(:));
obs_bitv = zeros(1, 2*ss);
obs_bitv(bnet.observed) = 1;
obs_bitv(bnet.observed+ss) = 1;

ns = bnet.node_sizes(:);
CPDpot = cell(ss,T);

for n=1:ss
  % slice 1
  t = 1;
  ps = parents(bnet.dag, n);
  e = bnet.equiv_class(n, 1);
  if ~any(obs_bitv(ps))
    CPDpot{n,t} = convert_CPD_to_table_hidden_ps(bnet.CPD{e}, evidence{n,t});
  else
    CPDpot{n,t} = convert_to_table(bnet.CPD{e}, [ps n], evidence(:,1));
  end

% special cases: c=child, p=parents, d=discrete, h=hidden, 1sl=1slice
% if c=h=1 then c=d=1, since hidden nodes must be discrete
% c=h c=d p=h p=d 1sl method
% ---------------------------
% 1   1   1   1   -   replicate CPT
% -   1   -   1   -   evaluate CPT on evidence *
% 0   1   1   1   1   dhmm
% 0   0   1   1   1   ghmm
% other               loop
%
% * = any subset of the domain may be observed

% Example where all of the special cases occur - a hierarchical HMM
% where the top layer (G) and leaves (Y) are observed and
% all nodes are discrete except Y.
% (O turns on if Y is an outlier)

% G ---------> G
% |            |
% v            v
% S  --------> S
% |            |
% v            v
% Y            Y
% ^            ^
% |            |
% O            O

% Evaluating P(yt|St,Ot) is the ghmm case
% Evaluating P(St|S(t-1),gt) is the eval CPT case
% Evaluating P(gt|g(t-1) is the eval CPT case (hdom = [])
% Evaluating P(Ot) is the replicated CPT case

% Cts parents (e.g., inputs) would require an additional special case for speed

  % slices 2..T
  [ss T] = size(evidence);
  self = n+ss;
  ps = parents(bnet.dag, self);
  e = bnet.equiv_class(n, 2);

  if 1
  debug = 0;
  hidden_child = ~obs_bitv(n);
  discrete_child = myismember(n, bnet.dnodes);
  hidden_ps = all(~obs_bitv(ps));
  discrete_ps = mysubset(ps, bnet.dnodes);
  parents_in_same_slice = all(ps > ss);

  if hidden_child & discrete_child & hidden_ps & discrete_ps
    CPDpot = helper_repl(bnet, evidence, n, CPDpot, obs_bitv, debug);
  elseif discrete_child & discrete_ps
    CPDpot = helper_eval(bnet, evidence, n, CPDpot, obs_bitv, debug);
  elseif discrete_child & hidden_ps & discrete_ps & parents_in_same_slice
    CPDpot = helper_dhmm(bnet, evidence, n, CPDpot, obs_bitv, debug);
  elseif ~discrete_child & hidden_ps & discrete_ps & parents_in_same_slice
    CPDpot = helper_ghmm(bnet, evidence, n, CPDpot, obs_bitv, debug);
  else
    if debug, fprintf('node %d, slow\n', n); end
    for t=2:T
      CPDpot{n,t} = convert_to_table(bnet.CPD{e}, [ps self], evidence(:,t-1:t));
    end
  end
  end

  if 0
  for t=2:T
    CPDpot2{n,t} = convert_to_table(bnet.CPD{e}, [ps self], evidence(:,t-1:t));
    if ~approxeq(CPDpot{n,t}, CPDpot2{n,t})
      fprintf('CPDpot n=%d, t=%d\n',n,t);
      keyboard
    end
  end
  end

end

%%%%%%%
function CPDpot = helper_repl(bnet, evidence, n, CPDpot, obs_bitv, debug)

[ss T] = size(evidence);
if debug, fprintf('node %d, repl\n', n); end
e = bnet.equiv_class(n, 2);
CPT = convert_CPD_to_table_hidden_ps(bnet.CPD{e}, []);
CPDpot(n,2:T) = num2cell(repmat(CPT, [1 1 T-1]), [1 2]);

%%%%%%%
function CPDpot = helper_eval(bnet, evidence, n, CPDpot, obs_bitv, debug)

[ss T] = size(evidence);
self = n+ss;
ps = parents(bnet.dag, self);
e = bnet.equiv_class(n, 2);
ns = bnet.node_sizes(:);
% Example: given CPT(p1, p2, p3, p4, c), where p1,p3 are observed
% we create CPT([p2 p4 c], [p1 p3]).
% We then convert all observed p1,p3 into indices ndx
% and return CPT(:, ndx)
CPT = CPD_to_CPT(bnet.CPD{e});
domain = [ps self];
% if dom is [3 7 8] and 3,8 are observed, odom_rel = [1 3], hdom_rel = 2,
% odom = [3 8], hdom = 7
odom_rel = find(obs_bitv(domain));
hdom_rel = find(~obs_bitv(domain));
odom = domain(odom_rel);
hdom = domain(hdom_rel);
if isempty(hdom)
  CPT = CPT(:);
else
  CPT = permute(CPT, [hdom_rel odom_rel]);
  CPT = reshape(CPT, prod(ns(hdom)), prod(ns(odom)));
end
parents_in_same_slice = all(ps > ss);
if parents_in_same_slice
  if debug, fprintf('node %d eval 1 slice\n', n); end
  data = cell2num(evidence(odom-ss,2:T)); %data(i,t) = val of i'th obs parent at t+1
else
  if debug, fprintf('node %d eval 2 slice\n', n); end
  % there's probably a way of vectorizing this...
  data = zeros(length(odom), T-1);
  for t=2:T
    ev = evidence(:,t-1:t);
    ev = ev(:);
    ev2 = ev(odom);
    data(:,t-1) = cat(1, ev2{:});
    %data(:,t-1) = cell2num(ev2);
  end
end
ndx = subv2ind(ns(odom), data'); % ndx(t) encodes data(:,t)
if isempty(hdom)
  CPDpot(n,2:T) = num2cell(CPT(ndx)); % a cell array of floats
else
  CPDpot(n,2:T) = num2cell(CPT(:, ndx), 1); % a cell array of column vectors
end

%%%%%%%
function CPDpot = helper_dhmm(bnet, evidence, n, CPDpot, obs_bitv, debug)

if debug, fprintf('node %d, dhmm\n', n); end
[ss T] = size(evidence);
self = n+ss;
ps = parents(bnet.dag, self);
e = bnet.equiv_class(n, 2);
ns = bnet.node_sizes(:);
CPT = CPD_to_CPT(bnet.CPD{e});
CPT = reshape(CPT, [prod(ns(ps)) ns(self)]); % what if no parents?
%obslik = mk_dhmm_obs_lik(cell2num(evidence(n,2:T)), CPT);
obslik = eval_pdf_cond_multinomial(cell2num(evidence(n,2:T)), CPT);
CPDpot(n,2:T) = num2cell(obslik, 1);

%%%%%%%
function CPDpot = helper_ghmm(bnet, evidence, n, CPDpot, obs_bitv, debug)

if debug, fprintf('node %d, ghmm\n', n); end
[ss T] = size(evidence);
e = bnet.equiv_class(n, 2);
S = struct(bnet.CPD{e});
ev2 = cell2num(evidence(n,2:T));
%obslik = mk_ghmm_obs_lik(ev2, S.mean, S.cov);
obslik = eval_pdf_cond_gauss(ev2, S.mean, S.cov);
CPDpot(n,2:T) = num2cell(obslik, 1);


function t = typeof(a)
%TYPEOF       Type of a
%
%   t = typeof(a)
%
%For details, see intval\typeof and intval\typeadj.
%

% written  05/22/09     S.M. Rump  taylor added
%

% taylor\@taylor\typeof:  a  must be taylor
  if isa(a.t,'intval')
    t = 'taylorintval';
  else
    t = 'taylor';
  end


%                           benchmarkSynthetic.m
%
% A general framework for benchmarking different phase retrieval algorithms
% using synthetic signals and either synthetic or real meaurement matrices.
%
% I/O
% Inputs
% xitem:  A string that describes the desired x-axis label of the plot that
%         that is produced by this benchmark.  Valid options are
%         ['m/n', 'snr','iterations', 'time'].  When using a 2D image with
%         Fourier measurements, then 'masks' should be used instead of
%         'm/n' to conntrol the number of Fourier masks.
% xvalues: A list of scalar values for which the performance of each
%          algorithm is measured.
% yitem:  A string to appear as the y-axis label.  Value should be
%         drawn from ['reconError', 'measurementError', 'correlation'];
% algorithms: a cell array of options structs,
%             where each struct is the same as the input parameter 'opts'
%             for solvePhaseRetrieval. See the example scripts for details.
% dataSet: The name of dataset used. Currently supported options are
%          ['1DGaussian', '2DImage', 'transmissionMatrix'].
%
% params: a struct of options containing the following fields:
%          verbose(boolean, default=false):
%                   If true, the result of each trial will be reported.
%          numTrials(integer, default=1):
%                   The number of trials each algorithm/dataset
%                   combination will run.
%          policy(string, default='median'):
%                   How to compute the final yvalue used for ploting from
%                   the values one gets by running numTrials trials. It
%                   currently supports
%                   ['median','mean','best','successRate'].
%          successConstant(real number,defualt=1e-5):
%                   If the yvalue of the current  trial is less than this,
%                   the trial will be counted as a success. This parameter
%                   will only be used when policy='successRate'.
%          maxTime(positive real number,default=120) :
%                   Max time allowed for a single algorithm.
%          recordSignals(boolean, default=false):
%                   Whether to record the recovered signal at each trial.
%
%
% Outputs
%          results : A 3D struct consisting of the errors(error
%                   metric is based on yitem chosen by the user) of all
%                   trials of algorithm/dataset combinations. Coordinates
%                   are (x-axis value, algorithm index, trial number).
% recoveredSignals: A 4D cell array consisting of the recovered signal at
%                   each trial for each algorithm. Coordinates are (x-axis
%                   value, algorithm index, current trial number, the index
%                   of the recovered signal).
%
% For more details, please look at the Phasepack User Guide.
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

% Main benchmark interface
function [finalResults, results, recoveredSignals] = benchmarkSynthetic(xitem, xvalues, yitem, algorithms, dataSet, params)

% Check if the inputs are valid. Note: the fields of algorithms will be checked by
% solvePhaseRetrieval implicitly.
checkValidityOfInputs(xitem, xvalues, yitem, dataSet);

% If params is not provided, create it.
if ~exist('params', 'var')
    params = struct;
end

% Provide default value/validate the params provided by the user.
params = manageOptionsForBenchmark(dataSet, params);

% Check if the params are valid for the dataSet chosen by the user.
params = checkValidityOfParams(dataSet, params, xitem);

% Get the labels for x, y axis, numTrials, policy, successConstant,
% and recordSignals.
% For details of what these represent, see the header in this file or the User Guide.
numTrials = params.numTrials;
policy = params.policy;
successConstant = params.successConstant;
recordSignals = params.recordSignals;

% create struct to store results of each trial.
results = zeros(length(xvalues), length(algorithms), numTrials);
if recordSignals
    recoveredSignals = cell(length(xvalues), length(algorithms), numTrials);
end

fprintf('Benchmarking on dataset %s with %s as x axis and the %s\n %s of %d trials as y axis...\n\n',dataSet,xitem,policy,yitem,numTrials);

% Loop over the xvalues
for p=1:length(xvalues)
    fprintf('Running trails: %s=%g\n',xitem,xvalues(p))
    % Loop over the algorithms
    for k=1:length(algorithms)
        [opts,params] = setupTrialParameters(algorithms{k},xitem,xvalues(p),dataSet,params);
        fprintf('  %s:',algorithms{k}.algorithm);
        if numTrials==1
            fprintf('\n');
        end
        % Loop over the random trials
        for q=1:numTrials
            if numTrials>1 && params.verbose==0
                fprintf('*');
            end

            % Create a random test problem with the right dimensions and SNR
            [A, At, b0, xt, plotter, params] = createProblemData(dataSet, params);
            n = numel(xt);
            opts.xt = xt;

            % Call the solver specified in opts
            startTime = tic;
            [x, outs, opts] = solvePhaseRetrieval(A, At, b0, n, opts);
            elapsedTime = toc(startTime);

            % Update plot
            plotter(x);
            title(sprintf('%s (%s=%s)',opts.algorithm,xitem,num2str(xvalues(p))),'fontsize',16);
            drawnow();

            % Calculate the value the user is looking for.
            yvalue = evalY(yitem, x, xt, A, b0);
            % Store the value for this trial in a table.
            results(p, k, q) = yvalue;
            if recordSignals
                recoveredSignals{p,k,q} = x;
            end

            % Report results of a trial if verbose is true.
            if params.verbose
                reportResult(opts.initMethod, opts.algorithm, xitem, xvalues(p),...
                    yitem, yvalue, elapsedTime, q);
            end
        end
        fprintf('\n');
    end
end

% Get final results in order to plot a comparison graph for algorithms
% at different x values.
finalResults = getFinalResults(results, policy, successConstant, yitem);

% Plot the comparison of performance graph among different chosen algorithms and
% initial methods combinations.
plotComparison(xitem,yitem,xvalues,algorithms,dataSet,finalResults,policy);

end

%% Helper functions

% Set the parameters needed to solve a phase retrieval problem with the
% specified dataset, xtime, and yitem.
function [opts,params] = setupTrialParameters(opts,xitem,xval,dataSet,params)
switch lower(xitem)
    case 'iterations'
        opts.maxIters = xval;    % Update algorithm's max iterations.
        opts.tol = 1e-10;              % Set algorithm's tolerance to a very
        % small number in order to make it
        % run maxIters iterations.
        opts.maxTime = params.maxTime; % Set a time limit for each single trial.
        opts.isComplex = params.isComplex;
        opts.isNonNegativeOnly = params.isNonNegativeOnly;
    case 'm/n'
        if strcmp(lower(dataSet),'1dgaussian') || strcmp(lower(dataSet),'transmissionmatrix')
            params.m = round(params.n * xval); % Update m according m/n ratio.
        end
        opts.maxTime = params.maxTime; % Set a time limit for each single trial
    case 'snr'
        opts.maxTime = params.maxTime; % Set a time limit for each single trial.
        params.snr = xval;
    case 'time'
        opts.maxTime = xval;   % Set a time limit for each single trial.
        opts.tol = 1e-10;            % Set algorithm's tolerance to be very small
        opts.maxIters = 1e8;         % and algorithm's maxIters to be very large so it runs maxTime.
        opts.recordTimes = false;    % To save space, record nothing
        opts.recordResiduals = false;
    case 'masks'
        params.numMasks = xval;
        opts.maxTime = params.maxTime; % Set a time limit for each single trial
    case 'angle'
        opts.maxTime = params.maxTime; % Set a time limit for each single trial.
        opts.initMethod = 'angle';
        opts.initAngle = xval;
    otherwise
        % Raise an error if the given label for x axis is invalid.
        error(['invalid x label: ' xitem]);
end
end

% Run a specified algorithm on a specific dataset and get results
% Inputs:
% dataSet and params are as defined in benchmarkPhaseRetrieval
% opts: a struct consists of options specified by user for running the algorithm
%       on a specified dataset.
% Outputs:
% x:  n x 1 vector, estimation of xt given by the PR algorithm the subrountine invokes.
% xt: n x 1 vector, the real unknown signal.
% A:  m x n matrix/function handle(n x 1 -> m x 1).
% b0: m x 1 vector, the measurement.
% opts: a struct consists of options finally used for running the algorithm on a specified
%       dataset.
function [A, At, b0, xt, plotter, params] = createProblemData(dataSet, params)
    switch lower(dataSet)
        case '1dgaussian'
            [A, At, b0, xt, plotter] = experimentGaussian1D(params.n, params.m,...
                params.isComplex, params.isNonNegativeOnly);
        case '2dimage'
            [A, At, b0, xt, plotter] = experimentImage2D(params.numMasks, params.imagePath);
        case 'transmissionmatrix'
            [A, b0, xt, plotter] = experimentTransMatrixWithSynthSignal(params.n, params.m, params.A_cached);
            params.A_cached = A;
            At = [];
        otherwise
            error('unknown dataset: %s\n',dataSet);
    end

    % Add noise to achieve specified SNR
    if params.snr ~= inf
        noise = randn(params.m,1); % create noise
        noise = noise/norm(noise)*norm(b0)/params.snr;
        b0 = max(b0+noise,0);
    end

end

% Calculate how good the solution is. Use the metric specified by yitem.
function yvalue=evalY(yitem, x, xt, A, b0)
    %  Compute optimal rotation
    switch lower(yitem)
        case 'reconerror'
            % solve for least-squares solution:  alpha*x = xt
            alpha = (x(:)'*xt(:))/(x(:)'*x(:));
            x = alpha*x;
            yvalue = norm(xt(:)-x(:))/norm(xt(:));
        case 'measurementerror'
            % Transform A into function handle if A is a matrix
            if isnumeric(A)
                At = @(x) A'*x;
                A = @(x) A*x;
            end
            yvalue = norm(abs(A(x))-b0(:))/norm(b0(:));
        case 'correlation'
            yvalue = abs(x'*xt/norm(x)/norm(xt));
        otherwise
            error(['invalid y label: ' yitem]);
    end
end

% Get final results by averaging across all trials.
% The possible policies are the following:
% mean: take the mean of all trials.
% best: take the best of all trials. If yitem=='reconerror' or 'measurementerror',
%       min value will be taken; If yitem=='correlation', max value will be taken.
% median: take the median of all trials.
% successrate: A success rate will be calculated. If yitem=='reconerror' or
%              'measurementerror', it is the percentage of values that are
%              smaller than the successConstant. If yitem=='correlation', it is
%              the percentage of values that are larger than the successConstant.
% The input struct results has size length(xvalues) x length(algorithms) x numTrials
% The output struct finalResults has size length(xvalues) x length(algorithms)
function finalResults = getFinalResults(results, policy, successConstant, yitem)
    switch lower(policy)
        case 'mean'
            finalResults = mean(results,3);
        case 'best'
            switch lower(yitem)
                case {'reconerror', 'measurementerror'}
                    finalResults = min(results,[],3);
                case 'correlation'
                    finalResults = max(results,[],3);
                otherwise
                    error('invalid yitem: %s',yitem);
            end
        case 'median'
            finalResults = median(results,3);
        case 'successrate'
            switch lower(yitem)
                case {'reconerror', 'measurementerror'}
                    finalResults = mean(results<successConstant,3);
                case 'correlation'
                    finalResults = mean(results>successConstant,3);
                otherwise
                    error('invalid yitem: %s',yitem);
            end
        otherwise
            error('Invalid policy: %s', policy);
    end
end

% Plot a performance curve for each algorithm
function plotComparison(xitem,yitem,xvalues,algorithms,dataSet,finalResults,policy)
    algNames = {};
    % get the labels to appear in the legend
    for k=1:length(algorithms)
        if isfield(algorithms{k},'label') && numel(algorithms{k}.label)>0             % use user-specified label if available
            algNames{k} =  algorithms{k}.label;
        else
            algNames{k} = algorithms{k}.algorithm;  % otherwise use the algorithm name
        end
    end

    autoplot(xvalues,finalResults,algNames);

    title(char(strcat({xitem},{' VS '},{yitem},{' on '},{dataSet})));                         % title for plot
    xlabel(xitem);                                       % x-axis label
    ylabel(char(strcat({policy},{'('},{yitem},{')'})));  % y-axis label
    ax = gca;                                           % create an axes object
    ax.FontSize = 12;                                   % adjust font size on the axes
    legend('show', 'Location', 'northeastoutside');     % show the legend
end

% Report the result at the end of each trial if verbose is true.
function reportResult(initMethod, algorithm, xitem, xvalue, yitem, yvalue, time, currentTrialNum)
    if currentTrialNum==1
        fprintf('\n');
    end
    fprintf('Trial: %d, initial method: %s, algorithm: %s, %s: %d, %s: %f, time: %f\n',...
        currentTrialNum, initMethod, algorithm, xitem, xvalue, yitem, yvalue, time);
end

% Check if the inputs are valid.
function checkValidityOfInputs(xitem, xvalues, yitem, dataSet)
    assert(~isempty(xvalues), 'The list xvalues must not be empty.');

    % Check if yitem is a valid choice
    yitemList = {'reconerror', 'measurementerror', 'correlation'};
    checkIfInList('yitem', yitem, yitemList);

    % Check if dataSet chosen supports xitem
    xitem = lower(xitem);
    switch lower(dataSet)
        case '1dgaussian'
            supportedList = {'iterations','m/n','snr','time','angle'};
        case 'transmissionmatrix'
            supportedList = {'iterations','m/n','snr','time','angle'};
        case '2dimage'
            supportedList = {'masks','iterations','angle'};
        otherwise
            error('unknown dataset: %s\n',dataSet);
    end
    checkIfInList('xitem',xitem,supportedList,strcat('For ',lower(dataSet),', '));
end

% Check if the params are valid
function params = checkValidityOfParams(dataSet, params, xitem)
    switch lower(dataSet)
        case '1dgaussian'
            assert(isfield(params,'n'),'User must specify signal dimension in params.n');
            checkIfNumber('params.n',params.n);
            if ~strcmp(xitem,'m/n')
                assert(isfield(params,'m'), ['User must specify the number of measurements in params.m when using xlabel ',xitem]);
            end
        case '2dimage'
            assert(~strcmp(xitem,'m/n'),'xlabel m/n is not supported when using 2D images.  Instead use "masks"');
            if ~(xitem=='masks')
                assert(exist('params.numMasks'), ['User must specify the number of Fourier masks in params.numMasks when using xlabel ',xitem]);
            end
        case 'transmissionmatrix'
            params.A_cached = [];
            assert(isfield(params,'n'),'User must specify signal dimension in params.n.  Options are {256,1600,4096} for the tranmissionMatrix dataset.');
            assert(params.n==256 || params.n==1600 || params.n==4096,...
                ['Invalid choice (',num2str(params.n),') ofr params.n for the transmissionMatrix dataset. Valid choices are {256,1600,4096}.'])
        otherwise
            error('unknown dataset: %s\n',dataSet);
    end

    if ~isfield(params,'snr')
        params.snr = Inf;
    end

end


function S_scalar2d = poynting(varargin)

chkarg(nargin == 5 || nargin == 7, 'five or seven arguments are required.')

iarg = 0;
iarg = iarg + 1; polarization = varargin{iarg};
chkarg(istypesizeof(polarization, 'Axis'), '"argument %d should be "polarization" (instance of Axis).', iarg);

if istypesizeof(varargin{iarg+1}, 'Scalar3d')
	iarg = iarg + 1; Ep3d = varargin{iarg};
	chkarg(istypesizeof(Ep3d, 'Scalar3d'), '"argument %d should be "Ep3d" (instance of Scalar3d)."', iarg);

	iarg = iarg + 1; Eq3d = varargin{iarg};
	chkarg(istypesizeof(Eq3d, 'Scalar3d'), '"argument %d should be "Eq3d" (instance of Scalar3d)."', iarg);

	iarg = iarg + 1; Hp3d = varargin{iarg};
	chkarg(istypesizeof(Hp3d, 'Scalar3d'), '"argument %d should be "Hp3d" (instance of Scalar3d)."', iarg);

	iarg = iarg + 1; Hq3d = varargin{iarg};
	chkarg(istypesizeof(Hq3d, 'Scalar3d'), '"argument %d should be "Hq3d" (instance of Scalar3d)."', iarg);

	iarg = iarg + 1; normal_axis = varargin{iarg};
	chkarg(istypesizeof(normal_axis, 'Axis'), 'argument %d should be "normal_axis" (instance of Axis).', iarg);

	iarg = iarg + 1; intercept = varargin{iarg};
	chkarg(istypesizeof(intercept, 'real'), 'argument %d should be "intercept" (real).', iarg);

	chkarg(isequal(Ep3d.grid3d, Eq3d.grid3d, Hp3d.grid3d, Hq3d.grid3d), ...
		'instances of Scalar3d do not have same grid3d.');

	% This makes the Poynting vector calculation independnet of whether the
	% primary grid is the E-field grid or H-field grid, when normal_axis is
	% normal to p and q.
	Ep2d = slice_scalar3d(Ep3d, normal_axis, intercept);
	Eq2d = slice_scalar3d(Eq3d, normal_axis, intercept);
	Hp2d = slice_scalar3d(Hp3d, normal_axis, intercept);
	Hq2d = slice_scalar3d(Hq3d, normal_axis, intercept);

	grid2d = Ep2d.grid2d;
else
	iarg = iarg + 1; Ep2d = varargin{iarg};
	chkarg(istypesizeof(Ep2d, 'Scalar2d'), '"argument %d should be "Ep2d" (instance of Scalar2d)."', iarg);

	iarg = iarg + 1; Eq2d = varargin{iarg};
	chkarg(istypesizeof(Eq2d, 'Scalar2d'), '"argument %d should be "Eq2d" (instance of Scalar2d)."', iarg);

	iarg = iarg + 1; Hp2d = varargin{iarg};
	chkarg(istypesizeof(Hp2d, 'Scalar2d'), '"argument %d should be "Hp2d" (instance of Scalar2d)."', iarg);

	iarg = iarg + 1; Hq2d = varargin{iarg};
	chkarg(istypesizeof(Hq2d, 'Scalar2d'), '"argument %d should be "Hq2d" (instance of Scalar2d)."', iarg);

	chkarg(isequal(Ep2d.grid2d, Eq2d.grid2d, Hp2d.grid2d, Hq2d.grid2d), ...
		'instances of Scalar2d do not have same grid2d.');  % Grid2d has normal_axis, so passing this test implies shared normal axis
	chkarg(isequal(Ep2d.intercept, Eq2d.intercept, Hp2d.intercept, Hq2d.intercept), ...
		'instances of Scalar2d do not have same intercept.');

	grid2d = Ep2d.grid2d;
	normal_axis = grid2d.normal_axis;
	intercept = Ep2d.intercept;
end

if polarization == normal_axis
	assert(all(Ep2d.gt_array==Hq2d.gt_array) && all(Eq2d.gt_array==Hp2d.gt_array));

	pi = grid2d.l{Dir.h,GT.dual};
	qi = grid2d.l{Dir.v,GT.dual};
	[PI, QI] = ndgrid(pi, qi);  % centers of grid cell faces

	[ep, l1] = Ep2d.data_expanded();
	hq = Hq2d.data_expanded();  % hq and ep have same location
	[eq, l2] = Eq2d.data_expanded();
	hp = Hp2d.data_expanded();  % hp and eq have same location

	sr1 = ep .* conj(hq);
	sr2 = eq .* conj(hp);

	[P1, Q1] = ndgrid(l1{:});  % locations of sr1
	[P2, Q2] = ndgrid(l2{:});  % locations of sr2

	% Interpolate sr1 and sr2 at the centers of grid cell faces.
	sr1 = interpn(P1, Q1, sr1, PI, QI);
	sr2 = interpn(P2, Q2, sr2, PI, QI);

	array = real(sr1 - sr2) / 2;

	% Attach extra points.
	for d = Dir.elems
		array = attach_extra_S(array, d, grid2d);
	end

	osc = Ep2d.osc;
	physQ = PhysQ.S;
	gt_array = [GT.dual, GT.dual];  % face centers
else  % polarization ~= normal_axis
	% To be implemented.  See an attempted usage in example/2d/pc_2d_basic.m.
	% Note that in this case, Ep and Hq are not co-located.  Neither are Eq and
	% Hp.  (Draw their locations, and compare it with Ew2d.gt_array and
	% Hw2d.gt_array.)
end

% The attached values to array should be the same as the ones inside the array
% if BC is not periodic.
S_scalar2d = Scalar2d(array, grid2d, gt_array, osc, physQ, ['<', physQ.symbol, '_', char(polarization), '>'], intercept);

function array = attach_extra_S(array, d, grid2d)
ind_n = {':', ':'};
ind_p = {':', ':'};
bc_d = grid2d.bc(d);
if bc_d == BC.p
	ind_n{d} = grid2d.N(d);
	ind_p{d} = 1;
else  % bc_d == BC.e or BC.m
	ind_n{d} = 1;
	ind_p{d} = grid2d.N(d);
end
array = cat(int(d), array(ind_n{:}), array, array(ind_p{:}));  % Bloch phases in S are ignored due to conj(H)


%CODEGENERATOR.GENSLBLOCKCORIOLIS Generate Simulink block for Coriolis matrix
%
% cGen.genslblockcoriolis() generates a robot-specific Simulink block to compute
% Coriolis/centripetal matrix.
%
% Notes::
% - Is called by CodeGenerator.gencoriolis if cGen has active flag genslblock
% - The Coriolis matrix is stored row by row to avoid memory issues.
% - The Simulink block recombines the the individual blocks for each row.
% - Access to generated function is provided via subclass of SerialLink
%   whose class definition is stored in cGen.robjpath.
%
% Author::
%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
%
% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis.

% Copyright (C) 2012-2014, by Joern Malzahn
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
%
% The code generation module emerged during the work on a project funded by
% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully
% acknowledge the financial support.
function [ ] = genslblockcoriolis( CGen )

%% Open or create block library
bdclose('all')                                                              % avoid problems with previously loaded libraries
load_system('simulink');
if ~(exist([CGen.slibpath,simulinkext]) == 2)                                  % Create new block library if none exists
 CGen.createnewblocklibrary;
end
open_system(CGen.slibpath);
set_param(CGen.slib,'lock','off');

[q,qd] = CGen.rob.gencoords;

%% Generate Coriolis Block
CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the robot Coriolis matrix\n']);
nJoints = CGen.rob.n;
symname = 'coriolis';

CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']);
CoriolisBlock = [CGen.slib,'/',symname];
if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname))                    % Delete previously generated block
    delete_block(CoriolisBlock)
    save_system;
end

% Subsystem in which individual rows are concatenated
add_block('built-in/SubSystem',CoriolisBlock);                               % Add new inertia matrix block
add_block('simulink/Math Operations/Matrix Concatenate'...
    , [CoriolisBlock,'/coriolis']...
    , 'NumInputs',num2str(nJoints)...
    , 'ConcatenateDimension','1');
add_block('simulink/Sinks/Out1',[CoriolisBlock,'/out']);
add_block('simulink/Sources/In1',[CoriolisBlock,'/q']);
add_block('simulink/Sources/In1',[CoriolisBlock,'/qd']);
add_line(CoriolisBlock,'coriolis/1','out/1');
CGen.logmsg('\t%s\n',' done!');

for kJoints = 1:nJoints
    CGen.logmsg([datestr(now),'\t\t... Embedded Matlab Function Block for joint ',num2str(kJoints),': ']);

    % Generate Embedded Matlab Function block for each row
    symname = ['coriolis_row_',num2str(kJoints)];
    fname = fullfile(CGen.sympath,[symname,'.mat']);

    if exist(fname,'file')
        tmpStruct = load(fname);
    else
        error ('genslblockcoriolis:SymbolicsNotFound','Save symbolic expressions to disk first!')
    end

    blockaddress = [CoriolisBlock,'/',symname];
    if doesblockexist(CGen.slib,symname)
        delete_block(blockaddress);
        save_system;
    end

    CGen.logmsg('%s',' block creation');
    symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q,qd});

    % connect output
    CGen.logmsg('%s',', output wiring');
    if ( verLessThan('matlab','7.11.0.584') ) && ( isequal(tmpStruct.(symname),zeros(1,nJoints)) )
        % There is a bug in earlier Matlab versions. If the symbolic
        % vector is a zero vector, then the Simulink Embedded Matlab
        % Function block outputs a scalar zero. We need to concatenate
        % a row vector of zeros here, which we have to construct on our
        % own.
        add_block('simulink/Math Operations/Matrix Concatenate'...              % Use a matrix concatenation block ...
            , [CoriolisBlock,'/DimCorrection',num2str(kJoints)]...               % ... named with the current row number ...
            , 'NumInputs',num2str(nJoints),'ConcatenateDimension','2');         % ... intended to concatenate zero values for each joint ...
        % ... columnwise. This will circumvent the bug.

        for iJoints = 1:nJoints                                                 % Connect signal lines from the created block (which outputs
            add_line(CoriolisBlock...                                            % a scalar zero in this case) with the bugfix block.
                , [symname,'/1']...
                , ['DimCorrection',num2str(kJoints),'/', num2str(iJoints)]);
        end

        add_line(CoriolisBlock,['DimCorrection',num2str(kJoints)...              % Connect the fixed row with other rows.
            , '/1'],['coriolis/', num2str(kJoints)]);

    else
        add_line(CoriolisBlock,[symname,'/1']...          % In case that no bug occurs, we can just connect the rows.
            , ['coriolis/', num2str(kJoints)]);
    end

    % Vector generalized joint values
    add_line(CoriolisBlock,'q/1',[symname,'/1']);
    % Vector generalized joint velocities
    add_line(CoriolisBlock,'qd/1',[symname,'/2']);

    CGen.logmsg('\t%s\n','row complete!');
end
addterms(CoriolisBlock); % Add terminators where needed
distributeblocks(CoriolisBlock);
CGen.logmsg([datestr(now),'\tCoriolis matrix block complete\n']);

%% Cleanup
% Arrange blocks
distributeblocks(CGen.slib);

% Lock, save and close library
set_param(CGen.slib,'lock','on');
save_system(CGen.slib,CGen.slibpath);
close_system(CGen.slib);

end


function [cnmfAnalysisOutput] = computeCnmfSignalExtractionPatch(inputMovie,numExpectedComponents,varargin)
	% Brapper function for CNMF, update for most recent versions.
	% Building off of demo_script.m in CNMF github repo
	% Most recent commit tested on: https://github.com/epnev/ca_source_extraction/commit/187bbdbe66bca466b83b81861b5601891a95b8d1
	% https://github.com/epnev/ca_source_extraction/blob/master/demo_script_class.m
	% Biafra Ahanonu
	% started: 2019.03.11
	% inputs
		% inputMovie - a string or a cell array of strings pointing to the movies to be analyzed (recommended). Else, [x y t] matrix where t = frames.
		% numExpectedComponents - number of expected components
	% outputs
		% cnmfAnalysisOutput - structure containing extractedImages and extractedSignals along with input parameters to the algorithm
	% READ BEFORE RUNNING
		% Get CVX from http://cvxr.com/cvx/doc/install.html
		% Run the below commands in Matlab after unzipping
		% cvx_setup
		% cvx_save_prefs (permanently stores settings)

	[cnmfAnalysisOutput] = ciapkg.signal_extraction.computeCnmfSignalExtractionPatch(inputMovie,numExpectedComponents,'passArgs', varargin);
end


function [rec,prec,ap] = VOCevaldet_imglistin(VOCopts,id,cls,imglist,draw, ovthresh)
tic;
if(~exist('ovthresh', 'var'))
	ovthresh=0.5;
end
% load test set

cp=sprintf(VOCopts.annocachepath,'newval');
if exist(cp,'file')
    fprintf('%s: pr: loading ground truth\n',cls);
    load(cp,'gtids','recs');
else
    [gtids,t]=textread(sprintf(VOCopts.imgsetpath,VOCopts.testset),'%s %d');
	gtids=gtids(ismember(gtids, imglist));
    for i=1:length(gtids)
        % display progress
        if toc>1
            fprintf('%s: pr: load: %d/%d\n',cls,i,length(gtids));
            drawnow;
            tic;
        end

        % read annotation
        recs(i)=PASreadrecord(sprintf(VOCopts.annopath,gtids{i}));
    end
    save(cp,'gtids','recs');
end

fprintf('%s: pr: evaluating detections\n',cls);

% hash image ids
%hash=VOChash_init(gtids);
hash = containers.Map;
for i = 1:length(gtids)
  hash(gtids{i}) = i;
end
% extract ground truth objects

npos=0;
gt(length(gtids))=struct('BB',[],'diff',[],'det',[]);
for i=1:length(gtids)
    % extract objects of class
    clsinds=strmatch(cls,{recs(i).objects(:).class},'exact');
    gt(i).BB=cat(1,recs(i).objects(clsinds).bbox)';
    gt(i).diff=[recs(i).objects(clsinds).difficult];
    gt(i).det=false(length(clsinds),1);
    npos=npos+sum(~gt(i).diff);
end

% load results
[ids,confidence,b1,b2,b3,b4]=textread(sprintf(VOCopts.detrespath,id,cls),'%s %f %f %f %f %f');
BB=[b1 b2 b3 b4]';

% sort detections by decreasing confidence
[sc,si]=sort(-confidence);
ids=ids(si);
BB=BB(:,si);

% assign detections to ground truth objects
nd=length(confidence);
tp=zeros(nd,1);
fp=zeros(nd,1);
tic;
for d=1:nd
    % display progress
    if toc>1
        fprintf('%s: pr: compute: %d/%d\n',cls,d,nd);
        drawnow;
        tic;
    end

    % find ground truth image
    %i=VOChash_lookup(hash,ids{d});
    i = hash(ids{d});
	if isempty(i)
        error('unrecognized image "%s"',ids{d});
    elseif length(i)>1
        error('multiple image "%s"',ids{d});
    end

    % assign detection to ground truth object if any
    bb=BB(:,d);
    ovmax=-inf;
    for j=1:size(gt(i).BB,2)
        bbgt=gt(i).BB(:,j);
        bi=[max(bb(1),bbgt(1)) ; max(bb(2),bbgt(2)) ; min(bb(3),bbgt(3)) ; min(bb(4),bbgt(4))];
        iw=bi(3)-bi(1)+1;
        ih=bi(4)-bi(2)+1;
        if iw>0 & ih>0
            % compute overlap as area of intersection / area of union
            ua=(bb(3)-bb(1)+1)*(bb(4)-bb(2)+1)+...
               (bbgt(3)-bbgt(1)+1)*(bbgt(4)-bbgt(2)+1)-...
               iw*ih;
            ov=iw*ih/ua;
            if ov>ovmax
                ovmax=ov;
                jmax=j;
            end
        end
    end
    % assign detection as true positive/don't care/false positive
    if ovmax>=ovthresh%VOCopts.minoverlap
        if ~gt(i).diff(jmax)
            if ~gt(i).det(jmax)
                tp(d)=1;            % true positive
		gt(i).det(jmax)=true;
            else
                fp(d)=1;            % false positive (multiple detection)
            end
        end
    else
        fp(d)=1;                    % false positive
    end
end

% compute precision/recall
fp=cumsum(fp);
tp=cumsum(tp);
rec=tp/npos;
prec=tp./(fp+tp);

ap=VOCap(rec,prec);

if draw
    % plot precision/recall
    plot(rec,prec,'-');
    grid;
    xlabel 'recall'
    ylabel 'precision'
    title(sprintf('class: %s, subset: %s, AP = %.3f',cls,VOCopts.testset,ap));
end


function test_issue1410

% MEM 6gb
% WALLTIME 00:20:00
% DEPENDENCY

load(dccnpath('/home/common/matlab/fieldtrip/data/test/latest/raw/meg/preproc_ctf151.mat'), 'data');
grad = data.grad;
clear data

%%

vol = [];
vol.o = [0 0 4];
vol.r = 12;
vol.unit = 'cm';
vol.type = 'singlesphere';

%%

cfg = [];
cfg.dip.pos = [0 0 9];
cfg.dip.mom = [1 0 0];
cfg.dip.unit = 'cm';
cfg.headmodel = vol;
cfg.grad = grad;
data = ft_dipolesimulation(cfg);

cfg = [];
timelock = ft_timelockanalysis(cfg, data);

cfg = [];
cfg.method = 'mtmfft';
cfg.taper = 'hanning';
cfg.output = 'fourier';
freq = ft_freqanalysis(cfg, data);

%%

tfdata = timelock;
method = 'lcmv';

cfg = [];
cfg.xgrid = -8:2:8;
cfg.ygrid = -8:2:8;
cfg.zgrid = (-8:2:8) + 4;
cfg.unit = 'cm';
cfg.headmodel = vol;
cfg.method = method;
cfg.keepleadfield = 'yes';
cfg.(method).keepfilter = 'yes';
cfg.(method).lambda = '10%';
cfg.channel = 'MEG';
source1 = ft_sourceanalysis(cfg, tfdata);

%%

% in the next code the labels are missing for the precomputed filters
% this should assume that they were computed with the same channel selection

cfg = [];
cfg.method = method;
cfg.sourcemodel.pos = source1.pos;
cfg.sourcemodel.filter = source1.avg.filter;
cfg.channel = 'MEG';
source2 = ft_sourceanalysis(cfg, tfdata);

clear source1 source2

%%
% this is mostly from http://www.fieldtriptoolbox.org/tutorial/beamformingextended/

clear all
load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/sensor_analysis/subjectK.mat'));

data_combined = ft_appenddata([], data_left, data_right);

cfg           = [];
cfg.toilim    = [-0.8 1.1];
cfg.minlength = 'maxperlen'; % this ensures all resulting trials are equal length
data          = ft_redefinetrial(cfg, data_combined);

[ftver, ftdir] = ft_version;

%%
% the first analysis is for visual gamma

cfg        = [];
cfg.toilim = [-0.8 0];
data_bsl   = ft_redefinetrial(cfg, data);

cfg.toilim = [0.3 1.1];
data_exp   = ft_redefinetrial(cfg, data);

cfg      = [];
data_cmb = ft_appenddata(cfg, data_bsl, data_exp);

% give a number to each trial: 0 = baseline, 1 = experimental condition
data_cmb.trialinfo = [zeros(length(data_bsl.trial), 1); ones(length(data_exp.trial), 1)];

cfg            = [];
cfg.method     = 'mtmfft';
cfg.output     = 'fourier';
cfg.keeptrials = 'yes';
cfg.tapsmofrq  = 15;
cfg.foi        = 55;
freq_cmb       = ft_freqanalysis(cfg, data_cmb);

cfg                = [];
cfg.trials         = freq_cmb.trialinfo == 0;
freq_bsl           = ft_selectdata(cfg, freq_cmb);

cfg.trials         = freq_cmb.trialinfo == 1;
freq_exp           = ft_selectdata(cfg, freq_cmb);

%%
% this is used both for visual gamma and beta coherence

load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer_extended/hdm.mat'))
load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer_extended/sourcemodel.mat'))

cfg             = [];
cfg.sourcemodel = sourcemodel;
cfg.headmodel   = hdm;
cfg.channel     = {'MEG'};
cfg.grad        = data.grad;
cfg.singleshell.batchsize = 10;
sourcemodel_lf  = ft_prepare_leadfield(cfg);

templatedir = fullfile(ftdir, 'template', 'sourcemodel');
template = load(fullfile(templatedir, 'standard_sourcemodel3d8mm')); % 8mm spacing grid

templatedir = fullfile(ftdir, 'external', 'spm8', 'templates');
template_mri = ft_read_mri(fullfile(templatedir, 'T1.nii'));
template_mri.coordsys = 'mni'; % we know it's in MNI space

%%

cfg                   = [];
cfg.frequency         = freq_cmb.freq;
cfg.grad              = freq_cmb.grad;
cfg.method            = 'dics';
cfg.keeptrials        = 'yes';
cfg.channel           = 'MEG';
cfg.sourcemodel       = sourcemodel_lf;
cfg.keeptrials        = 'yes';
cfg.dics.lambda       = '5%';
cfg.dics.keepfilter   = 'yes';
cfg.dics.fixedori     = 'yes';
cfg.dics.realfilter   = 'yes';
source                = ft_sourceanalysis(cfg, freq_cmb);

% beam pre- and poststim by using the common filter
cfg.sourcemodel.filter  = source.avg.filter;
cfg.sourcemodel.label   = source.avg.label;
source_bsl       = ft_sourceanalysis(cfg, freq_bsl);
source_exp       = ft_sourceanalysis(cfg, freq_exp);

cfg = [];
cfg.parameter = 'avg.pow';
cfg.operation = '(x1 ./ x2) - 1';
source_diff = ft_math(cfg, source_exp, source_bsl);

source_diff.pos = template.sourcemodel.pos;
source_diff.dim = template.sourcemodel.dim;

cfg              = [];
cfg.parameter    = 'pow';
cfg.interpmethod = 'nearest';
source_diff_int  = ft_sourceinterpolate(cfg, source_diff, template_mri);

cfg               = [];
cfg.method        = 'slice';
cfg.funparameter  = 'pow';
cfg.maskparameter = cfg.funparameter;
cfg.funcolorlim   = [0.0 1.2];
cfg.opacitylim    = [0.0 1.2];
cfg.opacitymap    = 'rampup';
ft_sourceplot(cfg, source_diff_int);

clear source_bsl source_exp source_diff source_diff_int

%%
% now for the EMG coherence

cfg                 = [];
cfg.toilim          = [-1 -0.0025];
cfg.minlength       = 'maxperlen'; % this ensures all resulting trials are equal length
data_stim           = ft_redefinetrial(cfg, data);

cfg                 = [];
cfg.output          = 'powandcsd';
cfg.method          = 'mtmfft';
cfg.taper           = 'dpss';
cfg.tapsmofrq       = 5;
cfg.foi             = 20;
cfg.keeptrials      = 'yes';
cfg.channel         = {'MEG' 'EMGlft' 'EMGrgt'};
cfg.channelcmb      = {'MEG' 'MEG'; 'MEG' 'EMGlft'; 'MEG' 'EMGrgt'};
freq_csd            = ft_freqanalysis(cfg, data_stim);

%%

cfg                 = [];
cfg.method          = 'dics';
cfg.refchan         = 'EMGlft';
cfg.channel         = 'MEG';
cfg.frequency       = 20;
% with precomputed leadfields
cfg.sourcemodel     = sourcemodel_lf;
source_coh_lft      = ft_sourceanalysis(cfg, freq_csd);

% without precomputed leadfields
cfg.sourcemodel     = removefields(sourcemodel_lf, {'leadfield', 'leadfielddimord', 'label'});
cfg.headmodel       = hdm;
source_coh          = ft_sourceanalysis(cfg, freq_csd);

figure; hold on
plot(source_coh.avg.coh(:), '.')
plot(source_coh_lft.avg.coh(:), 'ro')

assert(isalmostequal(source_coh_lft.avg.coh, source_coh.avg.coh, 'reltol', 1e-6));

%%

cfg                  = [];
cfg.method           = 'dics';
cfg.refchan          = 'EMGlft';
cfg.channel          = 'MEG';
cfg.frequency        = 20;
cfg.headmodel        = hdm;
cfg.sourcemodel.pos  = [3.6987   -3.1192   11.2426];
cfg.sourcemodel.unit = 'cm';
cfg.reducerank       = 2;
cfg.keepleadfield    = 'yes';
cfg.dics.keepfilter  = 'yes';
cfg.backproject      = 'yes';
source3              = ft_sourceanalysis(cfg, freq_csd);
cfg.backproject      = 'no';
source2              = ft_sourceanalysis(cfg, freq_csd);

assert(isalmostequal(source3.avg.coh, source2.avg.coh, 'reltol', 1e-6));
assert(~isequal(source3.leadfield, source2.leadfield)); % should have different number of columns
assert(~isequal(source3.avg.filter, source2.avg.filter)); % should have different number of rows

clear source3 source2

%%

source_coh_lft.pos = template.sourcemodel.pos;
source_coh_lft.dim = template.sourcemodel.dim;

source_coh.pos = template.sourcemodel.pos;
source_coh.dim = template.sourcemodel.dim;

cfg              = [];
cfg.parameter    = 'coh';
cfg.interpmethod = 'nearest';
cfg.coordsys     = 'mni';
source_coh_int   = ft_sourceinterpolate(cfg, source_coh_lft, template_mri);

cfg               = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'coh';
cfg.maskparameter = 'coh';
cfg.headmodel     = hdm;
ft_sourceplot(cfg, source_coh_int);

cfg               = [];
cfg.method        = 'surface';
cfg.funparameter  = 'coh'; % use it to represent color
cfg.maskparameter = 'coh'; % and use it for transparency
ft_sourceplot(cfg, source_coh_int);

%%
% let's also try dics_refdip

% these are approximately in the motor cortex
diplft = [4  3 10];
diprgt = [4 -3 10];

% take the nearest grid point
d = sqrt(sum((sourcemodel_lf.pos - repmat(diplft, size(sourcemodel_lf.pos,1), 1)).^2, 2));
[mind, indx] = min(d);
diplft = sourcemodel_lf.pos(indx,:);

cfg                 = [];
cfg.method          = 'dics';
cfg.refdip          = diplft;
cfg.channel         = 'MEG';
cfg.frequency       = 20;
% with precomputed leadfields
cfg.sourcemodel     = sourcemodel_lf; % the leadfields will be discarded
cfg.headmodel       = hdm;
source_coh_diplft   = ft_sourceanalysis(cfg, freq_csd);

cfg = [];
cfg.funparameter = 'coh'; % use it to represent color
ft_sourceplot(cfg, source_coh_diplft); % it should be 1 on the location of diplft


function DataMat = in_data_mat( DataFile )
% IN_DATA_MAT: Read EEG recordings stored in a free .MAT file.

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

%% ===== GET OPTIONS =====
% Get options
OPTIONS = bst_get('ImportEegRawOptions');

%% ===== READ MATRIX FILE =====
% Read file
RawMat = load(DataFile);
% Get fields
fields = fieldnames(RawMat);
validFields = {};
% Loop to find the possible fields
for i = 1:length(fields)
    if ~isempty(RawMat.(fields{i})) && isnumeric(RawMat.(fields{i})) && (numel(RawMat.(fields{i})) > 31)
        validFields{end+1} = fields{i};
    end
end
if isempty(validFields)
    DataMat = [];
    bst_error(['No valid recordings field in: "' DataFile '"'], 'Import EEG data', 0);
    return
end
% Ask user which field to use, if there is more than one
if (length(validFields) > 1)
    res = java_dialog('question', 'Please select the field that contains your EEG recordings:', ...
                      'Import EEG data', [], validFields);
    % If user did not answer: exit
    if isempty(res)
        DataMat = [];
        return
    end
else
    res = validFields{1};
end
% Use selected field
FileData = RawMat.(res);

% Check matrix orientation
switch OPTIONS.MatrixOrientation
    case 'channelXtime'
        % OK
    case 'timeXchannel'
        % Transpose needed
        FileData = permute(FileData, [2 1 3]);
end

% Build time vector
Time = ((0:size(FileData,2)-1) ./ OPTIONS.SamplingRate - OPTIONS.BaselineDuration);
% ChannelFlag
ChannelFlag = ones(size(FileData,1), 1);
% Apply voltage units (in Brainstorm: recordings are stored in Volts)
switch (OPTIONS.VoltageUnits)
    case '\muV'
        FileData = FileData * 1e-6;
    case 'mV'
        FileData = FileData * 1e-3;
    case 'V'
        % Nothing to change
    case 'None'
        % Nothing to change
end
% If only one time frame: double it
if (size(FileData, 2) == 1)
    FileData = repmat(FileData, [1 2 1]);
end

% If loading 3D matrix: 3rd dimension is the epoch list
nbEpoch = size(FileData, 3);

% Initialize returned structure
DataMat = db_template('datamat');
DataMat.Comment     = 'EEG/MAT';
DataMat.ChannelFlag = ChannelFlag;
DataMat.Time        = Time;
DataMat.Device      = 'Unknown';
DataMat.nAvg        = OPTIONS.nAvg;
DataMat = repmat(DataMat, [nbEpoch, 1]);

% Process each epoch
BaseComment = DataMat(1).Comment;
for i = 1:nbEpoch
    DataMat(i).F = double(FileData(:,:,i));
    % Add indice number for multiple epochs
    if (nbEpoch > 1)
        DataMat(i).Comment = sprintf('%s #%d', BaseComment, i);
    end
end


function img = colorSegments(cube, method)
% Transforms the segmentation mask into a color image. This  function is
% just intended as a visualization tool.
% It might introduce artifacts if you try to use this function for segmenting objects.

[nrows, ncols, ncolor] = size(cube);
cube = double(reshape(cube, [nrows*ncols ncolor]));

if nargin == 1
    % Sort segments from smaller to larger and visualize with occlusions
    area = squeeze(sum(cube, 1));
    [foo, k] = sort(-area);
    cube = cube(:,k);
end

mask = zeros([nrows*ncols 1]);
for i = ncolor:-1:1
    cube(:,i) = cube(:,i).*(1-mask);
    mask = mask+cube(:,i);
end

map = hsv(ncolor);
map = map(randperm(ncolor),:);

img = cube * map;
img = reshape(img, [nrows ncols 3]);
img(img>1) = 1;
img = uint8(255*img);


% load all result variables

width = 24;
height = 12;
fontsize = 9;
plot_filename = 'pie_household_02';
plot_folder = 'projects/seminar/';

experiments = {'app_dishwasher_r_0.1', ...
                'app_freezer_r_0.2', ...
                'app_fridge_r_0.1', ...
                'app_kettle_r_0.1', ...
                'app_stereo_r_0.2', ...
                'app_stove_r_1', ...
                'app_tv_r_0.2', ...
            };

inferred = zeros(1, length(experiments) + 4);
actual = zeros(1, length(experiments) + 3);
appliance_names = {};
for i = 1:length(experiments)
    experiment = experiments{i};

    filename = ['results/details/weiss/weiss_initial/', experiment, '/default/result1.mat'];
    load(filename);
    setup = result.setup;
    household = setup.household;
    dataset = setup.dataset;
    granularity = setup.granularity;

    appliance_name = result.appliance_names{1};
    appliance_names = [appliance_names, appliance_name];
    % inferred consumption
    inferred(i) = sum(result.consumption) / 3600;

    evaluation_days = result.evaluation_and_training_days{1};
    appliance_id = getApplianceID(appliance_name);
    appliance_consumption = read_plug_data(dataset, ...
                                    household, ...
                                    appliance_id, ...
                                    evaluation_days, ...
                                    granularity);
    actual_valid = appliance_consumption ~= -1;
    percentage_valid = sum(actual_valid) / length(appliance_consumption);
    actual(i) = sum(appliance_consumption(actual_valid)) / 3600 / percentage_valid;
end

total_consumption = read_smartmeter_data(dataset, ...
                                    num2str(household, '%02d'), ...
                                    evaluation_days, ...
                                    granularity, ...
                                    'powerallphases');

%% Laptop
num_days = size(evaluation_days,1);
laptop = num_days * 40 * 4;
idx = length(experiments) + 1;
inferred(idx) = laptop;
appliance_id = getApplianceID('Laptop');
appliance_consumption = read_plug_data(dataset, ...
                                household, ...
                                appliance_id, ...
                                evaluation_days, ...
                                granularity);
actual_valid = appliance_consumption ~= -1;
percentage_valid = sum(actual_valid) / length(appliance_consumption);
actual(idx) = sum(appliance_consumption(actual_valid)) / 3600 / percentage_valid;
appliance_names{idx} = 'Laptop';

%% Lamp
lamp = num_days * 100 * 1;
idx = length(experiments) + 2;
inferred(idx) = lamp;
appliance_id = getApplianceID('Lamp');
appliance_consumption = read_plug_data(dataset, ...
                                household, ...
                                appliance_id, ...
                                evaluation_days, ...
                                granularity);
actual_valid = appliance_consumption ~= -1;
percentage_valid = sum(actual_valid) / length(appliance_consumption);
actual(idx) = sum(appliance_consumption(actual_valid)) / 3600 / percentage_valid;
appliance_names{idx} = 'Lamp';

%% Standby
num_days = size(evaluation_days,1);
min_cons = zeros(1,num_days);
for i = 1:num_days
    % 1 p.m. to 5 p.m.
    idx_start = (i-1)*86400 + 1*3600;
    idx_stop = (i-1)*86400 + 5*3600;
    min_cons(i) = min(total_consumption(idx_start:idx_stop));
end
standby = median(min_cons) * length(total_consumption) / 3600;
inferred(length(experiments) + 3) = standby;
appliance_names_inferred = appliance_names;
appliance_names_inferred{length(experiments) + 3} = 'Standby';

%% Other
other_inferred = sum(total_consumption) / 3600 - sum(inferred);
inferred(length(experiments) + 4) = other_inferred;
other_actual = sum(total_consumption) / 3600 - sum(actual);
actual(length(experiments) + 3) = other_actual;
appliance_names_inferred{length(experiments)+4} = 'Other';
appliance_names{length(experiments)+3} = 'Other';

%% Change TV and Laptop for better plotting
tmp = appliance_names(7);
appliance_names(7) = appliance_names(8);
appliance_names(8) = tmp;
tmp = appliance_names_inferred(7);
appliance_names_inferred(7) = appliance_names_inferred(8);
appliance_names_inferred(8) = tmp;
tmp = actual(7);
actual(7) = actual(8);
actual(8) = tmp;
tmp = inferred(7);
inferred(7) = inferred(8);
inferred(8) = tmp;

%% Save as csv file
fid = fopen([plot_folder, 'actual.csv'], 'w');
fprintf(fid, 'label,consumption,share\n');
for i = 1:length(actual)
    fprintf(fid, '%s, %.2f, %.3f\n', appliance_names{i}, actual(i)/1000, actual(i) / sum(actual));
end
fclose(fid);
fid = fopen([plot_folder, 'inferred.csv'], 'w');
fprintf(fid, 'label,consumption,share\n');
for i = 1:length(inferred)
    fprintf(fid, '%s, %.2f, %.3f\n', appliance_names_inferred{i}, inferred(i)/1000, inferred(i) / sum(inferred));
end
fclose(fid);

%
% fig = figure;
% hold on;
% subplot(1,2,1);
% pie(actual);
% title('Ground truth');
% subplot(1,2,2);
% pie(inferred);
% labels = appliance_names_inferred;
% legend(labels);
% title('Inferred');
% fig = make_report_ready(fig, 'size', [width, height], 'fontsize', fontsize);
% % print('-depsc2', '-cmyk', '-r600', [plot_folder, filename, '.eps']); % if eps is needed
% saveas(fig, [plot_folder, plot_filename, '.png'], 'png');
% close(fig);


function z = GetVel()
%
%
persistent Velp Posp

if isempty(Posp)
  Posp = 0;
  Velp = 80;
end

dt = 0.1;

v  = 0 +  10*randn;

Posp = Posp + Velp*dt;     % true position
Velp = 80 + v;             % true speed

z = Velp;


classdef MergeMertens < handle
    %MERGEMERTENS  Merge exposure sequence to a single image
    %
    % Pixels are weighted using contrast, saturation and well-exposedness
    % measures, than images are combined using laplacian pyramids.
    %
    % The resulting image weight is constructed as weighted average of
    % contrast, saturation and well-exposedness measures.
    %
    % The resulting image doesn't require tonemapping and can be converted to
    % 8-bit image by multiplying by 255, but it's recommended to apply gamma
    % correction and/or linear tonemapping.
    %
    % For more information see [MK07].
    %
    % ## References
    % [MK07]:
    % > Tom Mertens, Jan Kautz, and Frank Van Reeth. "Exposure fusion".
    % > In Computer Graphics and Applications, 2007. PG'07. 15th Pacific
    % > Conference on, pages 382-390. IEEE, 2007.
    %
    % See also: cv.MergeDebevec, cv.MergeRobertson, makehdr
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % contrast measure weight
        ContrastWeight
        % saturation measure weight
        SaturationWeight
        % well-exposedness measure weight
        ExposureWeight
    end

    %% MergeMertens
    methods
        function this = MergeMertens(varargin)
            %MERGEMERTENS  Creates MergeMertens object
            %
            %     obj = cv.MergeMertens()
            %     obj = cv.MergeMertens('OptionName',optionValue, ...)
            %
            % ## Options
            % * __ContrastWeight__ default 1.0
            % * __SaturationWeight__ default 1.0
            % * __ExposureWeight__ default 0.0
            %
            % See also: cv.MergeMertens.process
            %
            this.id = MergeMertens_(0, 'new', varargin{:});
        end

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

    %% MergeExposures
    methods
        function dst = process(this, src)
            %PROCESS  Merges images
            %
            %     dst = obj.process(src)
            %
            % ## Input
            % * __src__ vector of input images (1- or 3-channels), all of the
            %   same size and `uint8` type.
            %
            % ## Output
            % * __dst__ result image, same size as `src{i}` and `single` type.
            %
            % See also: cv.MergeMertens.MergeMertens
            %
            dst = MergeMertens_(this.id, 'process', src);
        end
    end

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

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

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

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

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

    %% Getters/Setters
    methods
        function value = get.ContrastWeight(this)
            value = AlignMTB_(this.id, 'get', 'ContrastWeight');
        end
        function set.ContrastWeight(this, value)
            AlignMTB_(this.id, 'set', 'ContrastWeight', value);
        end

        function value = get.SaturationWeight(this)
            value = AlignMTB_(this.id, 'get', 'SaturationWeight');
        end
        function set.SaturationWeight(this, value)
            AlignMTB_(this.id, 'set', 'SaturationWeight', value);
        end

        function value = get.ExposureWeight(this)
            value = AlignMTB_(this.id, 'get', 'ExposureWeight');
        end
        function set.ExposureWeight(this, value)
            AlignMTB_(this.id, 'set', 'ExposureWeight', value);
        end
    end

end


% labelmePrepareData
DO_DB = 0;
DO_LABELS = 0;
DO_TRANSFER_BMAPS = 0;
GET_TRAINING_DATA = 1;

addpath('~/src/labelme');

set = 'train'; % train, test

basedir = '~/data/occlusion/labelme';
imdir = fullfile(basedir, set, 'Images');
anndir = fullfile(basedir, set, 'Annotations');
labeldir = fullfile(basedir, 'labels2');

if DO_DB
    outdir = basedir;
    db = LMdatabase(anndir);
    save(fullfile(outdir, [set '_db']), 'db');
end

if DO_LABELS
    filestr = '*.xml';
    maxsize = 1024;
    outdir = fullfile(basedir, 'labels3');
    ignoreparts = true;
    processDirectory(anndir, filestr, outdir, '_labels.mat', ...
        @processIm2LabelmeLabels, imdir, maxsize, ignoreparts);
%     load(fullfile(outdir, [set '_db']));
%     parfor k = 1:numel(db)
%         [lim{k}, objnames{k}] = annotation2labels(db(k).annotation, imdir, 1024);
%     end
%     save(fullfile(outdir, [set '_labels']), 'lim', 'objnames');
end

if DO_TRANSFER_BMAPS
    setOcclusionDirectories;
    %load(fullfile(basedir, [set '_db']));
    isFullyLabeled = false(size(db));

    for k = 1:numel(db)

        if mod(k, 100)==0
            disp(num2str(k));
        end
        try
            folder = db(k).annotation.folder;
            bn = strtok(db(k).annotation.filename, '.');
            occfn = fullfile(occdir, folder, [bn '_occlusion']);
            labelfn = fullfile(labeldir, folder, [bn '_labels']);
            load(occfn, 'bndinfo_all');
            load(labelfn, 'lim');

            if mean(lim(:)>0)>0.97 % fully labeled

                isFullyLabeled(k) = true;

                % resize object segmentation image to be same size as
                [imh, imw] = size(bndinfo_all{1}.wseg);
                lim = imresize(lim, [imh imw], 'nearest');

                % get distance of pixels from ground truth boundary
                gtmap = seg2bmap(lim, imw, imh);
                gtdist = bwdist(gtmap);
                maxdist = 0.01*sqrt(imh.^2+imw.^2);

                ne = bndinfo_all{1}.ne;
                labels{k} = false(ne, 1);
                labels_near{k} = false(ne, 1);

                [lab, labim, err] = transferRegionLabels(lim, double(bndinfo_all{1}.wseg));
                for k2 = 1:ne
                    LR = bndinfo_all{1}.edges.spLR(k2, :);
                    labels{k}(k2) = lab(LR(1))~=lab(LR(2));
                    labels_near{k}(k2) = labels{k}(k2) && ...
                        (mean(gtdist(bndinfo_all{1}.edges.indices{k2}))<maxdist);
                end
            end
        catch
            disp(lasterr);
        end
    end
    save(fullfile(traindir, 'tranferredBoundaryLabels.mat'), 'labels', 'labels_near', 'isFullyLabeled');

end

if GET_TRAINING_DATA

    %N = 1000; % number of pixels per image to sample

    %load(fullfile(basedir, [set '_db']));
    load(fullfile(traindir, 'tranferredBoundaryLabels.mat'));
    x = cell(numel(labels_near), 1);
    y = cell(numel(labels_near), 1);
    for k = 1:numel(labels_near)

        if mod(k, 100)==0
            disp(num2str(k));
        end

        if isFullyLabeled(k)

            folder = db(k).annotation.folder;
            bn = strtok(db(k).annotation.filename, '.');

            occfn = fullfile(occdir, folder, [bn '_occlusion']);
            load(occfn, 'bndinfo_all');
            y{k} = labels_near{k};
            y{k} = 2*y{k}-1; % convert to {-1, 1}

            % set label of pixels near border to 0
            brd = 10;
            ind  = getBoundaryCenterIndices(bndinfo_all{1});
            [yk, xk] = ind2sub(ind, occ.bndinfo.imsize(1:2));
            valid = (yk>brd) & (yk <= occ.bndinfo_all{1}.imsize(1)-brd) & ...
                (xk > brd) & (xk <=occ.bndinfo_all{1}.imsize(2)-brd);
            y{k}(~valid) = 0;

            occ.po_all = getOcclusionMaps(bndinfo_all);
            x{k} = getBoundaryEdgletFeatures(bndinfo_all, occ);
        end

    end
    x = cat(1, x{:});
    y = cat(1, y{:});
    featDescription = {'pocc1', 'pocc2', 'pocc3', 'pocc4', 'pocc_max'};
    save(fullfile(traindir, 'trainData.mat'), 'x', 'y', 'featDescription');
end


clear
close all

% for linux
mex WarpMesh.cpp -lGLU -lOSMesa -lGL
% for mac
%mex WarpMesh.cpp -lGLU -lOSMesa -I/opt/X11/include/ -L/opt/X11/lib/

load demo.mat

%{
P=[570.3422         0  320.0000 0
          0  570.3422  240.0000 0
          0         0    1.0000 0];
%}

XYZcamera = double(XYZcamera);

XYZcamera(:,:,1) = - XYZcamera(:,:,1);
XYZcamera(:,:,2) = XYZcamera(:,:,2);
XYZcamera(:,:,3) = XYZcamera(:,:,3);

[label,depth]=WarpMesh(P,640,480,XYZcamera);

label = label';
label = label(:,end:-1:1);
figure
imagesc(label)

depth = depth';
depth = depth(end:-1:1,end:-1:1);

z_near = 0.3;
z_far_ratio = 1.2;
depth = z_near./(1-double(depth)/2^32);
maxDepth = max(depth(abs(depth) < 100));

cropmask = (depth < z_near) | (depth > z_far_ratio * maxDepth);
depth(cropmask) = 0; %NaN;%z_far_ratio * maxDepth;

figure
imagesc(depth)


function figure_handle = PTKShow2DSlicesInOneFigure(viewer_panel_handle, orientation, skip_sices, reporting)
    % PTKShowAll2DSlicesInOneFigure. Creates a figure showing every slice from a
    %     MimViewerPanel object displayed in a grid
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    if nargin < 4
        reporting = CoreReportingDefault;
    end

    % Number of plots along x axis
    num_plots_x = 3;

    % Maximum number of plots along y axis
    max_plots_y = 4;

    figure_handle = figure;
    axis off;
    hold on;
    gap = 0.01;

    number_slices = viewer_panel_handle.BackgroundImage.ImageSize(orientation);

    % Ignore slices at beginning and end
    number_slices = number_slices - 2*skip_sices;

    max_images_on_page = num_plots_x*max_plots_y;
    slice_spacing = ceil(number_slices/max_images_on_page);
    number_slices_shown = floor(number_slices/slice_spacing);

    num_plots_y  = ceil(number_slices_shown/num_plots_x);

    panel_slice_number = [1, 1, 1];
    viewer_panel_handle.Orientation = orientation;
    figure_num = 0;

    for slice_num = skip_sices : slice_spacing : number_slices + skip_sices

        figure_num = figure_num + 1;

        % Make viewer display this slice
        panel_slice_number(orientation) = slice_num;
        viewer_panel_handle.SliceNumber = panel_slice_number;

        % Capture the frame (including overlay)
        frame = viewer_panel_handle.Capture;

        yc = floor((figure_num - 1)/num_plots_x);
        xc = mod(figure_num - 1, num_plots_x);
        y_pos = 1 - yc/num_plots_y;
        y_size =  1/num_plots_y;
        x_pos = xc/num_plots_x;
        x_size =  1/num_plots_x;
        im = frame2im(frame);

        % Make figure active
        figure(figure_handle);
        pos = [x_pos + gap, y_pos - y_size + gap, x_size - 2*gap, y_size - 2*gap];
        axes_handle = axes('Position', pos, 'Visible', 'off', 'DataAspectRatio', [1 1 1]);
        image(im);
        set(axes_handle, 'DataAspectRatio', [1 1 1])
        hold on;
        axis off;
        set(axes_handle, 'Units', 'normalized', 'OuterPosition', pos, 'Position', pos);
        set(axes_handle, 'Units', 'normalized', 'OuterPosition', pos, 'Position', pos);
    end

    % Choose a figure size that fils the vertical screen size, but with the
    % correct ratio
    display_dimensions = CoreSystemUtilities.GetMonitorDimensions;

    % Adjust to take into account toolbars etc.
    display_height = display_dimensions(2) - 300;
    proportional_width = display_height*(size(im,2)/size(im,1))*(num_plots_x/num_plots_y)*(1 - 2*gap*num_plots_y)/(1 - 2*gap*num_plots_x);

    set(figure_handle, 'Units', 'pixels', 'Position', [0 0 proportional_width, display_height]);
    set(figure_handle, 'PaperPositionMode', 'auto');
end


function test_ft_prepare_singleshell

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_headmodel_singleshell ft_prepare_headmodel ft_prepare_singleshell

% function to test ft_headmodel_singleshell. this function is called
% by ft_prepare_headmodel

% the function should work either on an input (segmented) mri, or it
% should have a description of the geometry in the input, or it should
% have a hdmfile (string) that specifies which file to read

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get the data which is needed

% read in the segmented mri
datadir = dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/beamformer');
load(fullfile(datadir, 'segmentedmri.mat'));
mri = segmentedmri; clear segmentedmri;

% specify the file for the headshape
hdmfile  = dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.shape');

% read in the headshape
shape = ft_read_headshape(hdmfile);

%%%%%%%%%%%%%%%%%%%%%
% do the computations

% with an MRI in the input
cfg         = [];
cfg.method  = 'singleshell';
vol1        = ft_prepare_headmodel(cfg, mri);

cfg         = [];
vol1b       = ft_prepare_singleshell(cfg, mri);

% the following needs to be done to be able to make the comparison
vol1 = rmfield(vol1, 'cfg');
vol1b = rmfield(vol1b,'cfg');
vol1bu=ft_convert_units(vol1b,vol1.unit);

% Reason for dashboard failure:
% e.g.  vol1.bnd.pnt(5,:) is different than vol1bu.bnd.pnt(5,:)
% e.g.  vol1.bnd.pnt(8,:) is different than vol1bu.bnd.pnt(8,:)
vol1.bnd    = rmfield(vol1.bnd, 'cfg');
vol1bu.bnd  = rmfield(vol1bu.bnd, 'cfg');

if ~isequal(vol1, vol1bu)
  error('ft_prepare_singleshell and ft_prepare_headmodel gave different outputs');
end

% with a filename in the input
cfg         = [];
cfg.method  = 'singleshell';
% cfg.headshape = hdmfile;
% vol2        = ft_prepare_headmodel(cfg);
vol2        = ft_prepare_headmodel(cfg, shape);

cfg         = [];
cfg.headshape = hdmfile;
vol2b       = ft_prepare_singleshell(cfg);

% the following needs to be done to be able to make the comparison
vol2  = rmfield(vol2, 'cfg');
vol2b = rmfield(vol2b,'cfg');

vol2.bnd  = rmfield(vol2.bnd, 'cfg');
vol2b.bnd = rmfield(vol2b.bnd,'cfg');

[ok, msg] = isalmostequal(vol2, vol2b, 'abstol', 1e-5, 'diffabs', 1);

if ~ok
  disp(msg);
  error('ft_prepare_singleshell and ft_prepare_headmodel gave different outputs');
end

% with a point cloud in the input
cfg         = [];
cfg.method  = 'singleshell';
cfg.geom    = shape;
vol3        = ft_prepare_headmodel(cfg);

cfg         = [];
cfg.headshape = shape;
vol3b       = ft_prepare_singleshell(cfg);

% the following needs to be done to be able to make the comparison
vol3  = rmfield(vol3, 'cfg');
vol3b = rmfield(vol3b,'cfg');

vol3.bnd  = rmfield(vol3.bnd, 'cfg');
vol3b.bnd = rmfield(vol3b.bnd,'cfg');

[ok, msg] = isalmostequal(vol3, vol3b, 'abstol', 1e-5, 'diffabs', 1);
if ~ok
  disp(msg);
  error('ft_prepare_singleshell and ft_prepare_headmodel gave different outputs');
end


function connectSparse = Graph2Connection(UNFOLD)
%
%   connectSparse = Graph2Connection(UNFOLD)
%
% AUTHOR:  Wandell
% DATE:   09.06.99
% PURPOSE:
%    Create a sparse matrix of the connections in a gray graph.
%
%

nodes = UNFOLD.nodes;
edges = UNFOLD.edges;

numNodes = size(nodes,2);
connectSparse = sparse(numNodes,numNodes);

for ii=1:numNodes
  l1 = mrUGetNeighbors(UNFOLD,ii);
  connectSparse(ii,l1) = 1;
end

return;

% Debug
%


function [X,Y,polygon] = UISelect

%UISelect - Interactively select polygon zone in an existing plot.
%
%  USAGE
%
%    [X,Y,p] = UISelect
%
%    X,Y            polygon coordinates
%    p              handle for polygon object in figure
%
%  SEE
%
%    See also UIInPolygon
%

% Copyright (C) 2009-2011 by Michaël Zugaro
%
% 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.

hold on;
X = [];
Y = [];
polygon = [];
last = false;

while ~last,
	% Get next mouse click
	[x,y,button] = ginput(1);
	switch button,
		case 1,
			% Left button adds this point
			X(end+1,1) = x;
			Y(end+1,1) = y;
		case 2,
			% Middle button closes the selection
			last = true;
			if ~isempty(X),
				X(end+1,1) = X(1);
				Y(end+1,1) = Y(1);
			end
		case 3,
			% Right button cancels last point
			if ~isempty(X),
				X(end) = [];
				Y(end) = [];
			end
	end
	% Update existing polygon
	if isempty(polygon),
		polygon = plot(X,Y);
	else
		warning('off','MATLAB:hg:line:XDataAndYDataLengthsMustBeEqual');
		set(polygon,'XData',X);
		set(polygon,'YData',Y);
		warning('on','MATLAB:hg:line:XDataAndYDataLengthsMustBeEqual');
	end
end

hold off;


function out = spm_dartel_warp(job)
% Register images to template data.
% format spm_dartel_warp(job)
%
% The outputs are flow fields.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_dartel_warp.m 4064 2010-09-03 12:57:10Z john $

code = 2;
st = job.settings;
n1 = numel(job.images);
n2 = numel(job.images{1});
NF = struct('NI',[],'vn',[1 1]);
NF(n1,n2) = struct('NI',[],'vn',[1 1]);

for i=1:n1,
    if numel(job.images{i}) ~= n2,
        error('Incompatible number of images');
    end;
    for j=1:n2,
        [pth,nam,ext,num] = spm_fileparts(job.images{i}{j});
        NF(i,j).NI        = nifti(fullfile(pth,[nam ext]));
        num               = [str2num(num) 1 1];
        NF(i,j).vn        = num(1:2);
    end;
end;

dm = [size(NF(1,1).NI.dat) 1];
dm = dm(1:3);

spm_progress_bar('Init',n2,'Registering');
numits = 0;
for it=1:numel(st.param),
    numits = numits + st.param(it).its;
end

for i=1:n2,

    f = zeros([dm,n1],'single');
    for j=1:n1,
        vn = NF(j,i).vn;
        f(:,:,:,j) = single(NF(j,i).NI.dat(:,:,:,vn(1),vn(2)));
    end
    f(~isfinite(f)) = 0;

    [pth,nam,ext] = fileparts(NF(1,i).NI.dat.fname);
    fprintf('*** %s ***\n', nam);

    NU = NF(1,i).NI;
    [pth,nam,ext]   = fileparts(NU.dat.fname);
    NU.dat.fname = fullfile(pth,['u_' nam '.nii']);
    NU.dat.dim   = [dm 1 3];
    NU.dat.dtype = 'float32-le';
    NU.dat.scl_slope = 1;
    NU.dat.scl_inter = 0;
    NU.descrip = 'Flow Field';
    if exist(NU.dat.fname,'file'),
        fprintf('Continuing registration from pre-existing parameters (%s)\n', NU.dat.fname);

        u = NU.dat(:,:,:,1,:);
        u = single(squeeze(u));
    else
        u = zeros([dm,3],'single');
    end;

    it0 = 0;
    for it=1:numel(st.param),
        param = st.param(it);
        prm   = [st.rform, param.rparam, st.optim.lmreg, ...
                 st.optim.cyc, st.optim.its, param.K, code];

        % Load the template
        NG = nifti(strvcat(param.template{:}));

        g  = squeeze(single(NG.dat(:,:,:,:,:)));
        if ~all(size(g)==size(f)),
            error('Incompatible dimensions between images and template');
        end
        for j=1:param.its,
            it0 = it0 + 1;
            [u,ll] = dartel3(u,f,g,prm);
            fprintf('%d \t%g\t%g\t%g\t%g\n',...
                it0,ll(1),ll(2),ll(1)+ll(2),ll(3));

            spm_progress_bar('Set',i-1 + it0/numits);
        end
    end
    create(NU);
    NU.dat(:,:,:,1,:) = reshape(u,[dm 1 3]);
end;
spm_progress_bar('Clear');

n2 = numel(job.images{1});
out.files = cell(n2,1);
for j=1:n2,
    [pth,nam,ext,num] = spm_fileparts(job.images{1}{j});
    fname             = fullfile(pth,['u_' nam '.nii']);
    out.files{j}      = fname;
end;


function [completed_data, predicted_label, energy] = rec_completion_test(model, test_data, mask, show, param)
% Given incomplete 3D shape(tsdf), recognition and completion test for
% multi-class model.

% Input the trained model, and some fixed masks, this function perform
% completion and recognition simultaneously. Returns the recognition
% accuracy for the incomplete data(test_data) and the completed_data as
% well as the free energy and predicted_label associated with that
% completion.

if ~exist('param','var');
    param.batch_size = 32;
    param.epochs = 50;
    param.gibbs_iter = 1;
    param.earlyStop = false;
end

addpath voxelization;
addpath 3D;
global kConv_backward  kConv_backward_c kConv_forward2 kConv_forward_c;

if ~isfield(model.layers{2},'uw')
    model = merge_model(model);
end

n = size(test_data,1);
num_layer = length(model.layers);
batch_size = 32;
batch_num = ceil(n / batch_size);

predicted_label = zeros(n, model.classes);
completed_data = initialize_missing(test_data, mask);

ncount = 0;
running_mean = zeros(1, model.classes);

for epoch = 1 : param.epochs
    for b = 1 : batch_num
        batch_end = min(n, b * batch_size);
        batch_data = completed_data((b-1) * batch_size+1 : batch_end, :,:,:,:);
        batch_label = predicted_label((b-1) * batch_size+1 : batch_end, :,:,:,:);
        batch_mask = mask((b-1) * batch_size+1 : batch_end,:,:,:);
        this_size = size(batch_data, 1);
        % propagate/inference bottum up using recognition weight.
        for l = 2 : num_layer - 1
            if l == 2
                hidden_presigmoid = myConvolve2(kConv_forward2, batch_data, model.layers{l}.uw, model.layers{l}.stride, 'forward');
                hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
                batch_hidden_prob = sigmoid(hidden_presigmoid);
            elseif strcmp(model.layers{l}.type, 'convolution')
                hidden_presigmoid = myConvolve(kConv_forward_c, batch_data, model.layers{l}.uw, model.layers{l}.stride, 'forward');
                hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
                batch_hidden_prob = sigmoid(hidden_presigmoid);
            else
                batch_data = reshape(batch_data, this_size, []);
                hidden_presigmoid = bsxfun(@plus, ...
                    batch_data * model.layers{l}.uw, model.layers{l}.c);
                batch_hidden_prob = 1 ./ ( 1 + exp(- hidden_presigmoid) );
            end
            batch_data = batch_hidden_prob;
        end

        batch_data = reshape(batch_data, this_size, []);
        % calculate the free energy for each label hypothesis
        %for c = 1 : model.classes
        %    try_label = zeros(batch_size, model.classes);
        %    try_label(:,c) = 1;
        %    batch_label(:,c) = free_energy(model, [try_label, batch_data], num_layer);
        %end

        hn_1 = batch_data;
        hn_1 = single(hn_1 > rand(size(hn_1)));

        temp_w = model.layers{num_layer}.w;
        temp_w(1:model.classes,:) = temp_w(1:model.classes,:) * model.duplicate;
        for i = 1 : param.gibbs_iter
            % alternating gibbs
            % prop up
            hn = bsxfun(@plus, [batch_label, hn_1] * temp_w, model.layers{num_layer}.c);
            hn = 1 ./ (1 + exp(-hn));
            hn = single(hn > rand(size(hn)));

            % prop down
            hn_1 = bsxfun(@plus, hn * model.layers{num_layer}.w', model.layers{num_layer}.b);
            batch_label = exp(bsxfun(@minus, hn_1(:,1:model.classes), max(hn_1(:,1:model.classes), [], 2)));
            batch_label = bsxfun(@rdivide, batch_label, sum(batch_label, 2));
            hn_1 = 1 ./ ( 1 + exp(-hn_1(:,model.classes+1:end)));
        end

        batch_data = reshape(hn_1, [this_size, model.layers{num_layer-1}.layerSize]);

        for l = num_layer - 1 : -1 : 2
            if l == 2
                batch_data = reshape(batch_data, [this_size, model.layers{l}.layerSize]);
                presigmoid = myConvolve(kConv_backward, batch_data, model.layers{l}.dw, model.layers{l}.stride, 'backward');
                presigmoid = bsxfun(@plus, presigmoid, permute(model.layers{l}.b, [5,1,2,3,4]));
                batch_data = 1 ./ ( 1 + exp(-presigmoid));
            elseif strcmp(model.layers{l}.type, 'convolution')
                batch_data = reshape(batch_data, [this_size, model.layers{l}.layerSize]);
                presigmoid = myConvolve(kConv_backward_c, batch_data, model.layers{l}.dw, model.layers{l}.stride, 'backward');
                presigmoid = bsxfun(@plus, presigmoid, permute(model.layers{l}.b, [5,1,2,3,4]));
                batch_data = 1 ./ ( 1 + exp(-presigmoid));
            else
                batch_data = reshape(batch_data, [this_size, model.layers{l}.layerSize]);
                presigmoid = bsxfun(@plus, ...
					batch_data * model.layers{l}.dw', model.layers{l}.b);
                batch_data = 1 ./ ( 1 + exp(-presigmoid) );
            end
        end
        % clamp the real data
        this_data = test_data((b-1) * batch_size + 1 : batch_end,:,:,:);
        batch_data(~batch_mask) = this_data(~batch_mask);

        completed_data((b-1) * batch_size + 1 : batch_end,:,:,:) = batch_data;
        predicted_label((b-1) * batch_size + 1 : batch_end,:) = batch_label;
    end

    if all(mean(predicted_label,1) == running_mean)
        ncount = ncount + 1;
    else
        running_mean = mean(predicted_label,1);
        ncount = 1;
    end
    % early stop
    if ((~param.earlyStop && epoch >= 100) || (param.earlyStop)) && ncount > 30 || epoch == param.epochs
        break;
    end
end

% calculate the free_energy
energy = zeros(n,1);
for b = 1 : batch_num
    batch_end = min(n, b * batch_size);
    batch_data = completed_data((b-1) * batch_size+1 : batch_end, :,:,:,:);
    batch_label = predicted_label((b-1) * batch_size+1 : batch_end, :,:,:,:);
    this_size = size(batch_data, 1);
    % propagate/inference bottum up using recognition weight.
    for l = 2 : num_layer - 1
        if l == 2
            hidden_presigmoid = myConvolve2(kConv_forward2, batch_data, model.layers{l}.uw, model.layers{l}.stride, 'forward');
            hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
            batch_hidden_prob = sigmoid(hidden_presigmoid);
        elseif strcmp(model.layers{l}.type, 'convolution')
            hidden_presigmoid = myConvolve(kConv_forward_c, batch_data, model.layers{l}.uw, model.layers{l}.stride, 'forward');
            hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
            batch_hidden_prob = sigmoid(hidden_presigmoid);
        else
            batch_data = reshape(batch_data, this_size, []);
            hidden_presigmoid = bsxfun(@plus, ...
                batch_data * model.layers{l}.uw, model.layers{l}.c);
            batch_hidden_prob = 1 ./ ( 1 + exp(- hidden_presigmoid) );
        end
        batch_data = batch_hidden_prob;
    end
    energy((b-1) * batch_size+1 : batch_end) = free_energy(model, [batch_label, batch_data], num_layer);
end

% show the result
if show
    for i = 1 : n
        the_sample = test_data(i,:,:,:,:);

        figure;
        subplot(1,2,1);
        plot3D(squeeze(completed_data(i,:,:,:,:)) > 0.1);
        p = patch(isosurface(squeeze(completed_data(i,:,:,:)),0.1));
        set(p,'FaceColor','red','EdgeColor','none');
        daspect([1,1,1])
        view(3); axis tight
        camlight
        lighting gouraud

        subplot(1,2,2);
        plot3D(squeeze(the_sample) > 0);
        p = patch(isosurface(squeeze(the_sample),0.1));
        set(p,'FaceColor','red','EdgeColor','none');
        daspect([1,1,1])
        view(3); axis tight
        camlight
        lighting gouraud
        pause;
        close(gcf);
    end
end

function data = initialize_missing(data, mask)
    n = size(data,1);
    bin  = 9;
    div = floor(n / bin); rev = mod(n,bin);
    rev_start = 5 - floor(rev / 2); rev_end = 5 + floor((rev-1) / 2);
    if rev == 0
        for i = 1 : bin
            this_data = data((i-1)*div+1:i*div,:,:,:);
            this_mask = mask((i-1)*div+1:i*div,:,:,:);
            this_data(this_mask) = single(rand(size(this_data(this_mask))) > (0.1 * i ));
            data((i-1)*div+1:i*div,:,:,:) = this_data;
        end
    else
        for i = 1 : rev_start-1
            this_data = data((i-1)*div+1:i*div,:,:,:);
            this_mask = mask((i-1)*div+1:i*div,:,:,:);
            this_data(this_mask) = single(rand(size(this_data(this_mask))) > (0.1 * i ));
            data((i-1)*div+1:i*div,:,:,:) = this_data;
        end
        np = (rev_start - 1) * div;
        for i = rev_start : rev_end
            this_data = data(np + (i-rev_start)*(div+1)+1 : np + (i-rev_start+1) * (div+1),:,:,:);
            this_mask = mask(np + (i-rev_start)*(div+1)+1 : np + (i-rev_start+1) * (div+1),:,:,:);
            this_data(this_mask) = single(rand(size(this_data(this_mask))) > 0.1 * i);
            data(np + (i-rev_start)*(div+1)+1 : np + (i-rev_start+1) * (div+1),:,:,:) = this_data;
        end
        np = (rev_start - 1) * div + rev * (div+1);
        for i = rev_end+1 : bin
            this_data = data(np + (i-rev_end-1)*div+1 : np + (i-rev_end) * div,:,:,:);
            this_mask = mask(np + (i-rev_end-1)*div+1 : np + (i-rev_end) * div,:,:,:);
            this_data(this_mask) = single(rand(size(this_data(this_mask))) > 0.1 * i);
            data(np + (i-rev_end-1)*div+1 : np + (i-rev_end) * div,:,:,:) = this_data;
        end
    end

function [y] = sigmoid(x)
	y = 1 ./ (1 + exp(-x));


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista@gmail.com
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista@gmail.com) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the target point positions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function targets = update_Target_Point_Positions(dt,current_time,targets)

%IDs = targets(:,1);                 % Stores Lag-Pt IDs in col vector
%xPts= targets(:,2);                 % Previous x-Values of x-Target Pts.
yPts= targets(:,3);                  % Previous y-Values of y-Target Pts.
%kStiffs = targets(:,4);             % Stores Target Stiffnesses
%N_target = length(targets(:,1));    % Gives total number of target pts!

%
% PUMPING PARAMETERS
%
freq = 2;     % pumping frequency
%G = 0.325;     % vertical gap between teeth
%occ = 0.95;   % occlusion total
a = 0.337;    % total vertical distance pump travels
N_top = 3276;  % # of points along top

%
% READ IN ORIGINAL yPT POSITIONS
%
yPTS = read_In_yPT_Positions('sawtooth.vertex');

%
% Create the pumping behavior
%
targets(1:N_top,3) = yPTS(1:N_top) - (a/2)*sin( 2*pi*freq*current_time );
%yPts(N_top+1:end) = yPts(N_top+1:end) + 0.95*(G/2)*sin( 2*pi*freq*current_time );

%
% Actually update target point positions
%
%targets(1:N_top,3) = yPts;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the # of vertex pts and all the vertex pts from the
%           .vertex file.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function PTS = read_In_yPT_Positions(struct_name)

filename = struct_name;  %Name of file to read in
fileID = fopen(filename);

% Read in the file, use 'CollectOutput' to gather all similar data together
% and 'CommentStyle' to to end and be able to skip lines in file.
C = textscan(fileID,'%f %f','CollectOutput',1);

fclose(fileID);     %Close the data file.

vertices = C{1};    %Stores all read in data in vertices (N+1,2) array

PTS = vertices(2:end,2);


function k = cmpndKernDiagCompute(kern, x)

% CMPNDKERNDIAGCOMPUTE Compute diagonal of CMPND kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the compound kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% SEEALSO : cmpndKernParamInit, kernDiagCompute, kernCreate, cmpndKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

i = 1;
if ~isempty(kern.comp{i}.index)
  % only part of the data is involved with the kernel.
  k  = kernDiagCompute(kern.comp{i}, x(:, kern.comp{i}.index));
else
  % all the data is involved with the kernel.
  k  = kernDiagCompute(kern.comp{i}, x);
end
for i = 2:length(kern.comp)
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved with the kernel.
    k  = k + kernDiagCompute(kern.comp{i}, x(:, kern.comp{i}.index));
  else
    % all the data is involved with the kernel.
    k  = k + kernDiagCompute(kern.comp{i}, x);
  end
end