%	OVERVIEW:
%       This demonstration analyzes a segment of data collected in the
%       intensive care unit (ICU) which contains ECG, ABP, and PPG signals
%
%   OUTPUT:
%       HRV Metrics exported to .cvs files
%
%   DEPENDENCIES & LIBRARIES:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   REFERENCE:
%       Vest et al. "An Open Source Benchmarked HRV Toolbox for Cardiovascular
%       Waveform and Interval Analysis" Physiological Measurement (In Press), 2018.
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Giulia Da Poian
%	COPYRIGHT (C) 2018
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; clc; close all;

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

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

HRVparams = InitializeHRVparams('demoICU'); % include the project name
HRVparams.poincare.on = 0; % Pinocare analysis off for this demo
HRVparams.DFA.on = 0; % DFA analysis off for this demo
HRVparams.MSE.on = 0; % MSE analysis off for this demo
HRVparams.HRT.on = 0; % HRT analysis off for this demo

[subjectIDs,filesTBA] = GenerateListOfFilesTBA(HRVparams.ext,HRVparams.readdata,0);

idx = find(strcmp(subjectIDs,'TestICUdata'));

i_patient = idx;

% 1. Load Raw Patient Data (ECG Waveform)
load(filesTBA{i_patient});

% 2. Analyze data using HRV PhysioNet Cardiovascular Signal Toolbox
[~, resFilename] = Main_HRV_Analysis(signal(:,1),[],'ECGWaveform',...
    HRVparams,subjectIDs(i_patient),[],[],signal(:,5),'PPG',signal(:,3),'ABP');

% 3. Load annotations ans SQI for plot
AnnFile = strcat(HRVparams.writedata, filesep, 'Annotation', filesep, ...
                                                    subjectIDs{i_patient});
jqrs_ann = read_ann(AnnFile,'jqrs');
[~,~,sqijw] = read_ann(AnnFile,'sqijw'); % SQI is x100 to be stored

ppg_ann = read_ann(AnnFile,'ppg');
qppg(signal(:,5),HRVparams.Fs);
[~,ppgsqi,ppgNumSqi] = read_ann(AnnFile,'sqippg');

abpann = read_ann(AnnFile,'abpm');
features =  abpfeature(signal(:,3), abpann, HRVparams.Fs);
[BeatQ, goodbeats] = jSQI(features, abpann, signal(:,3));

% 5. Plotting
HRVparams.gen_figs = 1;
if HRVparams.gen_figs
    % ECG
    Plot_SignalDetection_SQI(time, signal(:,1), jqrs_ann, sqijw'./100,'ECG')

    % ABP
    Plot_SignalDetection_SQI(time, signal(:,3), abpann, double(~BeatQ(:,1))','ABP')

    % RESP
    figure(88)
    plot(time,signal(:,6));
    title('Respiration')

    % PLETH
    Plot_SignalDetection_SQI(time, signal(:,5), ppg_ann, ppgNumSqi'./100,'PPG')

end

% 6. Pulse Transit Time
ptt = pulsetransit(jqrs_ann, abpann);

% 7. Plot BP vs PTT
syst = features(1:length(ptt),2);
if HRVparams.gen_figs
    figure(22)
    plot(syst,ptt(:,3)./HRVparams.Fs,'o');
    xlabel('BP (mmHg)'); ylabel('PTT (s)');
    title('Pulse Transit Time - BP vs PTT (ABP - QRS)')

end

% 8. Compare generated output file with the reference one

currentFile = strcat(HRVparams.writedata, filesep, resFilename.HRV, '.csv');
referenceFile = strcat('ReferenceOutput', filesep, 'ICU_HRV_allwindows.csv');
testHRV = CompareOutput(currentFile,referenceFile);

if testHRV
    fprintf('** DemoRawDataICU: TEST SUCCEEDED ** \n ')
     fprintf('A file named %s.csv \n has been saved in %s \n', ...
    resFilename.HRV, HRVparams.writedata);
else
    fprintf('** DemoRawDataICU: TEST FAILED ** \n')
end


function [varargout] = runopf_w_res(varargin)
%RUNOPF_W_RES  Runs an optimal power flow with fixed zonal reserves.
%   RESULTS = RUNOPF_W_RES(CASEDATA, MPOPT, FNAME, SOLVEDCASE)
%   [RESULTS, SUCCESS] = RUNOPF_W_RES(CASEDATA, MPOPT, FNAME, SOLVEDCASE)
%
%   Runs an optimal power flow with the addition of reserve requirements
%   specified as a set of fixed zonal reserves. See RUNOPF for a
%   description of the input and output arguments, which are the same,
%   with the exception that the case file or struct CASEDATA must define
%   a 'reserves' field, which is a struct with the following fields:
%       zones   nrz x ng, zone(i, j) = 1, if gen j belongs to zone i
%                                      0, otherwise
%       req     nrz x 1, zonal reserve requirement in MW
%       cost    (ng or ngr) x 1, cost of reserves in $/MW
%       qty     (ng or ngr) x 1, max quantity of reserves in MW (optional)
%   where nrz is the number of reserve zones and ngr is the number of
%   generators belonging to at least one reserve zone and ng is the total
%   number of generators.
%
%   In addition to the normal OPF output, the RESULTS struct contains a
%   new 'reserves' field with the following fields, in addition to those
%   provided in the input:
%       R       - ng x 1, reserves provided by each gen in MW
%       Rmin    - ng x 1, lower limit on reserves provided by each gen, (MW)
%       Rmax    - ng x 1, upper limit on reserves provided by each gen, (MW)
%       mu.l    - ng x 1, shadow price on reserve lower limit, ($/MW)
%       mu.u    - ng x 1, shadow price on reserve upper limit, ($/MW)
%       mu.Pmax - ng x 1, shadow price on Pg + R <= Pmax constraint, ($/MW)
%       prc     - ng x 1, reserve price for each gen equal to maximum of the
%                         shadow prices on the zonal requirement constraint
%                         for each zone the generator belongs to
%
%   See T_CASE30_USERFCNS for an example case file with fixed reserves,
%   and TOGGLE_RESERVES for the implementation.
%
%   Calling syntax options:
%       results = runopf_w_res(casedata);
%       results = runopf_w_res(casedata, mpopt);
%       results = runopf_w_res(casedata, mpopt, fname);
%       results = runopf_w_res(casedata, mpopt, fname, solvedcase);
%       [results, success] = runopf_w_res(...);
%
%   Example:
%       results = runopf_w_res('t_case30_userfcns');
%
%   See also RUNOPF, TOGGLE_RESERVES, T_CASE30_USERFCNS.

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

mpc = loadcase(varargin{1});
mpc = toggle_reserves(mpc, 'on');
[varargout{1:nargout}] = runopf(mpc, varargin{2:nargin});

if nargout > 0 && isstruct(varargout{1})
    varargout{1} = toggle_reserves(varargout{1}, 'off');
end


function xyzankurAnimCompare(X, X2, fps)

% XYZANKURANIMCOMPARE Animate a prediction and ground truth for stick man from Agarwal & Triggs dataset.
% FORMAT
% DESC animates a matrix of x,y,z point clound positions representing the
% motion of the figure used to generate the silhouttes for Agarwal &
% Triggs silhouette data.
% ARG y : the data to animate.
% ARG y2 : other data to animate.
% ARG fps : the number of frames per second to animate (defaults to 24).
%
% SEEALSO : xyzankurVisualise, xyzankurModify
%
% COPYRIGHT : Carl Henrik Ek and Neil D. Lawrence, 2008

% MOCAP

  if(nargin<3)
    fps = 24;
    if(nargin<2)
      fid = 1;
      if(nargin<1)
        error('Too few arguments');
      end
    end
  end
  clf
  for(i = 1:1:size(X,1))
    if(i==1)
      subplot(1, 2, 1)
      handle = xyzankurVisualise(X(i,:), 1);
      subplot(1, 2, 2)
      handle2 = xyzankurVisualise(X2(i,:), 1);
    else
      xyzankurModify(handle, X(i,:));
      xyzankurModify(handle2, X2(i,:));
    end
    pause(1/fps);
  end
end


function [pred, accuracy] = cosmo_crossvalidate(ds, classifier, partitions, opt)
% performs cross-validation using a classifier
%
% [pred, accuracy] = cosmo_crossvalidate(dataset, classifier, partitions, opt)
%
% Inputs
%   ds                  struct with fields .samples (PxQ for P samples and
%                       Q features) and .sa.targets (Px1 labels of samples)
%   classifier          function handle to classifier, e.g.
%                       @classify_naive_baysian
%   partitions          For example the output from nfold_partition
%   opt                 optional struct with options for classifier
%     .normalization    optional, one of 'zscore','demean','scale_unit'
%                       to normalize the data prior to classification using
%                       zscoring, demeaning or scaling to [-1,1] along the
%                       first dimension of ds. Normalization
%                       parameters are estimated using the training data
%                       and applied to the testing data.
%     .pca_explained_count   optional, transform the data with PCA prior to
%                            classification, and retain this number of
%                            components
%     .pca_explained_ratio   optional, transform the data with PCA prior to
%                            classification, and retain the components that
%                            explain this percentage of the variance
%                            (value between 0-1)
%    .check_partitions  optional (default: true). If set to false then
%                       partitions are not checked for being set properly.
%    .average_train_X   average the samples in the train set using
%                       cosmo_average_samples. For X, use any parameter
%                       supported by cosmo_average_samples, i.e. either
%                       'count' or 'ratio', and optionally, 'resamplings'
%                       or 'repeats'.
%
% Output
%   pred                Qx1 array with predicted class labels.
%                       elements with no predictions have the value NaN.
%   accuracy            scalar classification accuracy
%   test_chunks         Qx1 array with chunks of input dataset, if each
%                       prediction was based using a single classification
%                       step. Predictions with no or more than one
%                       classification step are set to NaN
%
% Examples:
%     % generate dataset with 3 targets and 4 chunks, first target is 3
%     ds=cosmo_synthetic_dataset('ntargets',3,'nchunks',4,'target1',3);
%     % use take-1-chunk for testing crossvalidation
%     partitions=cosmo_nfold_partitioner(ds);
%     classifier=@cosmo_classify_naive_bayes;
%     % run crossvalidation
%     [pred,accuracy]=cosmo_crossvalidate(ds, classifier, ...
%                                                       partitions);
%     % show targets, chunks, and predictions labels for each of the
%     % four folds
%     cosmo_disp({ds.sa.targets,ds.sa.chunks,pred},'threshold',inf)
%     %|| { [ 3    [ 1    [   3   NaN   NaN   NaN
%     %||     4      1        4   NaN   NaN   NaN
%     %||     5      1        5   NaN   NaN   NaN
%     %||     3      2      NaN     3   NaN   NaN
%     %||     4      2      NaN     5   NaN   NaN
%     %||     5      2      NaN     5   NaN   NaN
%     %||     3      3      NaN   NaN     3   NaN
%     %||     4      3      NaN   NaN     4   NaN
%     %||     5      3      NaN   NaN     5   NaN
%     %||     3      4      NaN   NaN   NaN     3
%     %||     4      4      NaN   NaN   NaN     4
%     %||     5 ]    4 ]    NaN   NaN   NaN     5 ] }
%     cosmo_disp(accuracy)
%     %||  0.917
%     %
%     % use take-2-chunks out for testing crossvalidation, LDA classifier
%     partitions=cosmo_nchoosek_partitioner(ds,2);
%     classifier=@cosmo_classify_lda;
%     % run crossvalidation
%     [pred,accuracy]=cosmo_crossvalidate(ds, classifier, ...
%                                                           partitions);
%     % show targets, chunks, and predictions labels for each of the
%     % four folds
%     cosmo_disp({ds.sa.targets,ds.sa.chunks,pred},'threshold',inf)
%     %|| { [ 3    [ 1    [   5     5     3   NaN   NaN   NaN
%     %||     4      1        4     4     4   NaN   NaN   NaN
%     %||     5      1        5     5     4   NaN   NaN   NaN
%     %||     3      2        3   NaN   NaN     3     3   NaN
%     %||     4      2        4   NaN   NaN     4     4   NaN
%     %||     5      2        5   NaN   NaN     4     5   NaN
%     %||     3      3      NaN     5   NaN     3   NaN     3
%     %||     4      3      NaN     4   NaN     4   NaN     4
%     %||     5      3      NaN     5   NaN     5   NaN     5
%     %||     3      4      NaN   NaN     3   NaN     3     3
%     %||     4      4      NaN   NaN     4   NaN     4     5
%     %||     5 ]    4 ]    NaN   NaN     5   NaN     3     3 ] }
%     cosmo_disp(accuracy)
%     %||   0.778
%     %
%     % as the example above, but (1) use z-scoring on each training set
%     % and apply the estimated mean and std to the test set, and (2)
%     % use odd-even partitioner
%     opt=struct();
%     opt.normalization='zscore';
%     partitions=cosmo_oddeven_partitioner(ds);
%     % run crossvalidation
%     [pred,accuracy]=cosmo_crossvalidate(ds, classifier, ...
%                                                        partitions, opt);
%     % show targets, predicted labels, and accuracy
%     cosmo_disp({ds.sa.targets,ds.sa.chunks,pred},'threshold',inf)
%     %|| { [ 3    [ 1    [ NaN     5
%     %||     4      1      NaN     4
%     %||     5      1      NaN     5
%     %||     3      2        3   NaN
%     %||     4      2        4   NaN
%     %||     5      2        5   NaN
%     %||     3      3      NaN     5
%     %||     4      3      NaN     4
%     %||     5      3      NaN     5
%     %||     3      4        3   NaN
%     %||     4      4        4   NaN
%     %||     5 ]    4 ]      3   NaN ] }
%     cosmo_disp(accuracy)
%     %||   0.75
%
% Notes:
%   - to apply this to a dataset struct as a measure (for searchlights),
%     consider using cosmo_crossvalidation_measure
%   - to average samples in the training set prior to training, use the
%     options provided by cosmo_average_samples prefixed by
%     'average_train_'. For example, to take averages of 5 samples, and use
%     each sample in the input approximately 4 times, use:
%           opt.average_train_count=5;
%           opt.average_train_resamplings=4;
%
% See also: cosmo_crossvalidation_measure, cosmo_average_samples
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    if nargin<4,opt=struct(); end
    if ~isfield(opt, 'normalization'),
        opt.normalization=[];
    end
    if ~isfield(opt, 'check_partitions'),
        opt.check_partitions=true;
    end

    if ~isempty(opt.normalization);
        normalization=opt.normalization;
        opt.autoscale=false; % disable for {matlab,lib}svm classifiers
    else
        normalization=[];
    end

    if all(isfield(opt, {'pca_explained_count','pca_explained_ratio'}))
            error(['pca_explained_count and pca_explained_ratio are ' ...
                'mutually exclusive'])
    elseif isfield(opt, 'pca_explained_count');
        arg_pca='pca_explained_count';
        arg_pca_value=opt.pca_explained_count;
    elseif isfield(opt, 'pca_explained_ratio');
        arg_pca='pca_explained_ratio';
        arg_pca_value=opt.pca_explained_ratio;
    else
        arg_pca=[];
    end

    if opt.check_partitions
        cosmo_check_partitions(partitions, ds);
    end

    % see if samples in training set are to be averaged
    [do_average_train, average_train_opt]=get_average_train_opt(opt);

    train_indices = partitions.train_indices;
    test_indices = partitions.test_indices;

    npartitions=numel(train_indices);

    nsamples=size(ds.samples,1);

    % space for output (one column per partition)
    % the k-th column contains predictions for the k-th fold
    % (with values of NaNs if there was no prediction)
    pred=NaN(nsamples,npartitions);

    targets=ds.sa.targets;

    % process each fold
    for fold=1:npartitions
        train_idxs=train_indices{fold};
        test_idxs=test_indices{fold};
        % for each partition get the training and test data, store in
        % train_data and test_data
        train_data = ds.samples(train_idxs,:);
        train_targets = targets(train_idxs);

        if do_average_train
            % make a minimal dataset, then use cosmo_average_samples to
            % compute averages
            n_train=numel(train_idxs);
            ds_train=struct();
            ds_train.samples=train_data;
            ds_train.sa.chunks=ones(n_train,1);
            ds_train.sa.targets=train_targets;

            ds_train_avg=cosmo_average_samples(ds_train,average_train_opt);
            train_data=ds_train_avg.samples;
            train_targets=ds_train_avg.sa.targets;
        end

        test_data = ds.samples(test_idxs,:);

        % apply pca
        if ~isempty(arg_pca)
            [train_data,pca_params]=cosmo_map_pca(train_data,...
                    arg_pca,arg_pca_value);
            test_data=cosmo_map_pca(test_data,'pca_params',pca_params);
        end

        % apply normalization
        if ~isempty(normalization)
            [train_data,params]=cosmo_normalize(train_data,normalization);
            test_data=cosmo_normalize(test_data,params);
        end

        % then get predictions for the training samples using
        % the classifier, and store these in the k-th column of all_pred.
        p = classifier(train_data, train_targets, test_data, opt);

        pred(test_idxs,fold) = p;
    end

    % compute accuracies
    has_prediction_mask=~isnan(pred);
    correct_mask=bsxfun(@eq,targets,pred) & has_prediction_mask;

    accuracy=sum(correct_mask)/sum(has_prediction_mask);

function [do_average_train, average_train_opt]=get_average_train_opt(opt)
    persistent cached_opt;
    persistent cached_do_average_train;
    persistent cached_average_train_opt;

    if ~isequal(cached_opt,opt)
        cached_average_train_opt=struct();
        cached_do_average_train=false;
        prefix='average_train_';
        keys=fieldnames(opt);
        for k=1:numel(keys)
            key=keys{k};
            sp=cosmo_strsplit(key,prefix);
            if isempty(sp{1})
                % starts with average_train, so take the rest of the key
                % and flag cached_do_average_train
                cached_average_train_opt.(sp{2})=opt.(key);
                cached_do_average_train=true;
            end
        end

        cached_opt=opt;
    end

    do_average_train=cached_do_average_train;
    average_train_opt=cached_average_train_opt;


function f = populate(f, op, data, pref)
%POPULATE   Populate a TRIGTECH class with values.
%   F = F.POPULATE(OP) returns a TRIGTECH representation populated with values
%   F.VALUES of the function OP evaluated on an equally spaced grid. The field
%   F.ISHAPPY indicates whether the representation is deemed 'happy'.
%   Essentially this means that such an interpolant is a sufficiently accurate
%   approximation to OP. If F.ISHAPPY is FALSE, then POPULATE was not able to
%   obtain a happy result.
%
%   OP should be vectorized (i.e., accept a vector input), and output a vector
%   of the same length. Furthermore, OP may be an array-valued function, in
%   which case it should accept a vector of length N and return a matrix of size
%   NxM.
%
%   F.POPULATE(OP, VSCALE, HSCALE) enforces that the happiness check is relative
%   to the initial vertical scale VSCALE and horizontal scale HSCALE. These
%   values default to 0 and 1 respectively. During refinement, VSCALE updates
%   itself to be the largest magnitude values to which (each of the columns in)
%   OP evaluated to.
%
%   F.POPULATE(OP, VSCALE, HSCALE, PREF) enforces any additional preferences
%   specified in the preference structure PREF (see TRIGTECH.TECHPREF).
%
%   F.POPULATE(VALUES, ...) (or F.POPULATE({VALUES, COEFFS}, ...)) populates F
%   non-adaptively with the VALUES (and COEFFS) passed. These values are still
%   tested for happiness in the same way as described above, but the length of
%   the representation is not altered.
%
% See also TRIGTECH, TECHPREF, HAPPINESSCHECK.

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

%%%%%%%%%%%%%%%%%%%%%%%%%% Non-adaptive construction. %%%%%%%%%%%%%%%%%%%%%%%%%%
% Values (and possibly coefficients) have been given.
if ( isnumeric(op) || iscell(op) )
    if ( isnumeric(op) )
        % OP is just the values.
        f.values = op;
        f.coeffs = f.vals2coeffs(op);
    else
        % OP is a cell {values, coeffs}
        f.coeffs = op{2};
        f.values = f.coeffs2vals(f.coeffs);
    end

    % We're always happy if given discrete data:
    f.ishappy = true;

    % Threshold to determine if f is real or not.
    vscl = max(data.vscale, vscale(f));
    f.isReal = max(abs(imag(f.values))) <= 3*(eps*vscl);
    f.values(:,f.isReal) = real(f.values(:,f.isReal));

    return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%% Adaptive construction. %%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialise empty values to pass to refine:
f.values = [];
% Check a random value of op in (-1,1) to see if the result is complex and
% if the value is a NaN or Inf.
pseudoRand = 0.376989633393435;
rndVal = feval(op, (2*pseudoRand - 1));

if ( any(isnan(rndVal)) || any(isinf(rndVal)) )
    error('CHEBFUN:TRIGTECH:populate:isNan','Cannot handle functions that evaluate to Inf or NaN.');
end

f.isReal = false(size(rndVal));
for k = 1:numel(rndVal)
    f.isReal(k) = isreal(rndVal(k));
end

% Loop until ISHAPPY or GIVEUP:
while ( 1 )

    % Call the appropriate refinement routine: (in PREF.REFINEMENTFUNCTION)
    [f.values, giveUp] = f.refine(op, f.values, pref);

    % We're giving up! :(
    if ( giveUp )
        break
    end

    % Update vertical scale: (Only include sampled finite values)
    valuesTemp = f.values;
    valuesTemp(~isfinite(f.values)) = 0;
    data.vscale = max(data.vscale, max(abs(valuesTemp)));

    % Compute the trigonometric coefficients:
    coeffs = f.vals2coeffs(f.values);

    % Check for happiness:
    f.coeffs = coeffs;
    [ishappy, cutoff] = happinessCheck(f, op, f.values, data, pref);

    % We're happy! :)
    if ( ishappy )
        f = prolong(f,cutoff); % chop coefficients to length cutoff
        break
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%% Assign to TRIGTECH object. %%%%%%%%%%%%%%%%%%%%%%%%%%
f.ishappy = ishappy;

% Force the values to be real if the imaginary part is zero
f.values(:,f.isReal) = real(f.values(:,f.isReal));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Ouput. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( ishappy )
    % We're done, and can return.
    return
end

end


%IM_BOX Fixed mapping determining a rectangular enclosing a blob (0/1 image)
%
%   B = IM_BOX(A)
%   B = A*IM_BOX
%
% If A is a 0/1 image then B is the same image with all empty (0) border
% columns and rows removed.
%
%   B = IM_BOX(A,N)
%
% If A is a 0/1 image then B is the same image, but having in each direction
% N empty (0) columns and rows around the object (1).
%
%   B = IM_BOX(A,[NX1 NX2 NY1 NY2])
%
% If A is a 0/1 image then B is the same image, but having NX1, NX2 empty
% columns (0) left, respectively right of the object (1) and NY1, NY2 empty
% rows (0) above, respectively below the object(1).
%
%   B = IM_BOX(A,N,ALF)
%
% Adds as many empty (0) columns or rows such that the aspect ratio of
% images (height/width) equals ALF. For ALF == 1, square images are returned.
% For ALF == 0, images are taken as they are and N rows and columns are
% added.
%
% EXAMPLE
% prdatafiles;            % make sure prdatafiles is in the path
% x = kimia_images;       % load kimia images
% x = x*im_box(0);        % remove all empty rows and columns
% x = x*im_box(0,1);      % add rows/columns to make images square
% x = x*im_resize([32 32]); % resample them
% x = x*im_box(1,0);      % add rows/columns and keep image square
% % now all images are 34x34 and no object touches the border.
% show(gendat(x,4))       % show a few
%
% SEE ALSO (PRTools Guide)
% DATASETS, DATAFILES

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [b,J] = im_box(varargin)

	argin = shiftargin(varargin,'vector');
  argin = setdefaults(argin,[],[],[]);
  if mapping_task(argin,'definition')
    b = define_mapping(argin,'fixed');
    b = setname(b,'Image bounding box');
  else
    [a,n,alf] = deal(argin{:});
    if isdataset(a)
      error('Command cannot be used for datasets as it may change image size')
    elseif isdatafile(a)
      isobjim(a);
      b = filtim(a,mfilename,{n,alf});
      %b = setfeatsize(b,getfeatsize(a));
    elseif isa(a,'double') || isa(a,'dip_image') % here we have a single image
      if isa(a,'dip_image'), a = double(a); end
      if isempty(n)
        jx = find(sum(a,1) ~= 0);
        jy = find(sum(a,2) ~= 0);
        J = [min(jy),max(jy),min(jx),max(jx)];
        b = a(min(jy):max(jy),min(jx):max(jx));
      else
        if (~isempty(alf) && alf == 0)
          c = a;
        else
          c = feval(mfilename,a);
        end
        [my,mx] = size(c);
        if length(n) == 1
        	n = [n n n n];
        elseif length(n) ~= 4
        	error('Second parameter should be scalar or vector of length 4')
        end
        b = zeros(my+n(3)+n(4),mx+n(1)+n(2));
        b(n(3)+1:n(3)+my,n(1)+1:n(1)+mx) = c;
      end
      if ~isempty(alf) && alf ~= 0
        [m,k] = size(b);
        r = round(m*alf) - k;
        if r == 0
          ;
        elseif r >= 1 % add r columns
          c = zeros(m,k+r);
          c(:,ceil(r/2):ceil(r/2)+k-1) = b;
          b = c;
        else % add rows
          r = round(k/alf) - m;
          c = zeros(m+r,k);
          c(ceil(r/2):ceil(r/2)+m-1,:) = b;
          b = c;
        end
      end
		end
	end

return


function out = cryptosubdecode(in,key)

numbers = double(lower(in))-96;
decodednumbers = zeros(1,length(numbers));
for i = 1:length(numbers)
    if numbers(i) >= 1 & numbers(i) <= 26
        decodednumbers(i) = find(key==numbers(i));
    else
        decodednumbers(i) = numbers(i);
    end
end

out = char(decodednumbers+96);


function [gt,dt] = evalRes( gt0, dt0, thr, mul )
% Evaluates detections against ground truth data.
%
% Uses modified Pascal criteria that allows for "ignore" regions. The
% Pascal criteria states that a ground truth bounding box (gtBb) and a
% detected bounding box (dtBb) match if their overlap area (oa):
%  oa(gtBb,dtBb) = area(intersect(gtBb,dtBb)) / area(union(gtBb,dtBb))
% is over a sufficient threshold (typically .5). In the modified criteria,
% the dtBb can match any subregion of a gtBb set to "ignore". Choosing
% gtBb' in gtBb that most closely matches dtBb can be done by using
% gtBb'=intersect(dtBb,gtBb). Computing oa(gtBb',dtBb) is equivalent to
%  oa'(gtBb,dtBb) = area(intersect(gtBb,dtBb)) / area(dtBb)
% For gtBb set to ignore the above formula for oa is used.
%
% Highest scoring detections are matched first. Matches to standard,
% (non-ignore) gtBb are preferred. Each dtBb and gtBb may be matched at
% most once, except for ignore-gtBb which can be matched multiple times.
% Unmatched dtBb are false-positives, unmatched gtBb are false-negatives.
% Each match between a dtBb and gtBb is a true-positive, except matches
% between dtBb and ignore-gtBb which do not affect the evaluation criteria.
%
% In addition to taking gt/dt results on a single image, evalRes() can take
% cell arrays of gt/dt bbs, in which case evaluation proceeds on each
% element. Use bbGt>loadAll() to load gt/dt for multiple images.
%
% Each gt/dt output row has a flag match that is either -1/0/1:
%  for gt: -1=ignore,  0=fn [unmatched],  1=tp [matched]
%  for dt: -1=ignore,  0=fp [unmatched],  1=tp [matched]
%
% USAGE
%  [gt, dt] = bbGt( 'evalRes', gt0, dt0, [thr], [mul] )
%
% INPUTS
%  gt0  - [mx5] ground truth array with rows [x y w h ignore]
%  dt0  - [nx5] detection results array with rows [x y w h score]
%  thr  - [.5] the threshold on oa for comparing two bbs
%  mul  - [0] if true allow multiple matches to each gt
%
% OUTPUTS
%  gt   - [mx5] ground truth results [x y w h match]
%  dt   - [nx6] detection results [x y w h score match]
%
% EXAMPLE
%
% See also bbGt, bbGt>compOas, bbGt>loadAll

% get parameters
if(nargin<3 || isempty(thr)), thr=.7; end
if(nargin<4 || isempty(mul)), mul=0; end

% if gt0 and dt0 are cell arrays run on each element in turn
if( iscell(gt0) && iscell(dt0) ), n=length(gt0);
  assert(length(dt0)==n); gt=cell(1,n); dt=gt;
  for i=1:n, [gt{i},dt{i}] = evalRes(gt0{i},dt0{i},thr,mul); end; return;
end

% check inputs
if(isempty(gt0)), gt0=zeros(0,5); end
if(isempty(dt0)), dt0=zeros(0,5); end
assert( size(dt0,2)==5 ); nd=size(dt0,1);
assert( size(gt0,2)==5 ); ng=size(gt0,1);

% sort dt highest score first, sort gt ignore last
[~,ord]=sort(dt0(:,5),'descend'); dt0=dt0(ord,:);
[~,ord]=sort(gt0(:,5),'ascend'); gt0=gt0(ord,:);
gt=gt0;  dt=dt0; dt=[dt zeros(nd,1)];
gt(:,5)=-gt(:,5);
% Attempt to match each (sorted) dt to each (sorted) gt
oa = compOas( dt(:,1:4), gt(:,1:4), gt(:,5)==-1 );
for d = 1:nd
  bstOa = thr;
  bstg = 0;
  bstm = 0; % info about best match so far
  for g = 1:ng
    % if this gt already matched, continue to next gt
    m = gt(g,5);
    if(m==1 && ~mul)
        continue;
    end
    % if dt already matched, and on ignore gt, nothing more to do
    if(bstm~=0 && m==-1)
        break;
    end
    % compute overlap area, continue to next gt unless better match made
    if(oa(d,g)<bstOa)
        continue;
    end
    % match successful and best so far, store appropriately
    bstOa = oa(d,g);
    bstg = g;
    if(m==0)
        bstm = 1;
    else
        bstm = -1;
    end
  end;
  g = bstg;
  m = bstm;
  % store type of match for both dt and gt
  if(m==-1)
      dt(d,6) = m;
  elseif(m==1)
      gt(g,5) = m;
      dt(d,6) = m;
  end
end

end


function predData = predictData(rez, samps)

W = gather_try(rez.W);
U = gather_try(rez.U);

samps = samps(1):samps(end);
buff = size(W,1); % width of a spike in samples

predData = zeros(size(U,1),numel(samps)+buff*4);

spikeTimes = rez.st3(:,1); % use the original spikes st2, not the merged and split

inclSpikes = spikeTimes>samps(1)-buff/2 & spikeTimes<samps(end)+buff/2;
st = spikeTimes(inclSpikes); % in samples

inclTemps = uint32(rez.st3(inclSpikes,2)); % use the original spikes st2, not the merged and split
amplitudes = rez.st3(inclSpikes,3);

for s = 1:sum(inclSpikes)
    ampi = amplitudes(s);
    Wi = sq(W(:, inclTemps(s), :));
    Ui = sq(U(:, inclTemps(s), :));

    % this is the reconstruction of the spatial part

    theseSamps = st(s)+(1:buff)-19-samps(1)+buff*2;
    predData(:,theseSamps) = predData(:,theseSamps) + Ui * Wi' * ampi;

    %     predData(:,theseSamps) = predData(:,theseSamps) + ...
    %         squeeze(U(:,inclTemps(s),:)) * squeeze(W(:,inclTemps(s),:))' * amplitudes(s);
end

predData = predData(:,buff*2+1:end-buff*2).*rez.ops.scaleproc;


function outState = FDSUpdateMap(state, newMap)
%         enums
        NEW = -1;
        OPEN = 1;
        CLOSED = 0;

        startPos = state.startPos;
        endPos =  state.endPos;
        pattern = state.pattern;
        ucc = state.ucc;
        graph = state.graph;
        kM = state.kM;
        SQRT2 = sqrt(2)-1;
        stack = state.stack;

        comparator = pcmp;
        stack2= java.util.PriorityQueue(180247, comparator);
        comparator = pcmp;
        stack3= java.util.PriorityQueue(180247, comparator);

        map = newMap;
        difference = map - state.map;
        state.map = map;
        %% ---------------addObstacle
        [x, y] = find(difference == -1);
        indices = [x,y];
        indices(:,3:4) = 0;

        Ind = [indices'];

        for n=indices'
            for s=state.pattern
                Ind = [Ind, n+s];
            end
        end

        added = unique(Ind', 'rows')';
        %% ---------------rmvObstacle
        [x, y] = find(difference == 1);
        indices = [x,y];
        indices(:,3:4) = 0;

        Ind = [indices'];

        for n=indices'
            for s=state.pattern
                Ind = [Ind, n+s];
            end
        end

        removed = unique(Ind', 'rows')';
        %% ----------------- process
        for u = removed
             tmp = inf;
                for n = ucc
                       s = u+n;
                            insert(s, h(s))
                end
        end
        for u = added
             tmp = inf;
                for n = ucc
                       s = u+n;
                        if t(s) == CLOSED && h(s) ~= inf
                            seth(s, inf);
                            s(3:4)  = [-k(s)+kM; k(s)+g(s)];
                            add(stack, s);
                            sett(s, OPEN);
                        end
                end
        end
        outState = state;
        outState.graph = graph;
        outState.map = map;
        outState.stack = stack;
%%
function out = inQ(s)
    out = graph(s(1), s(2), 3);
end
function out = t(s)
    out = graph(s(1), s(2), 3);
end
function sett(s, val)
    graph(s(1), s(2), 3) = val;
end
function setQ(s)
     graph(s(1), s(2), 3) = 1;
end
function rsetQ(s)
     graph(s(1), s(2), 3) = 0;
end
function incr(s)
    graph(s(1), s(2), 4) = graph(s(1), s(2), 4)+1;
end
%-----------------------------------------------------------
function out = g(s)
        s = abs(startPos - s);
%         s = abs(endPos - s);
%         out = SQRT2*min(s(1:2)) + max(s(1:2));
        out = sqrt(s(1)^2 + s(2)^2);
end
function out = h(s)
    out = graph(s(1), s(2),1);
end
function seth(s, val)
    graph(s(1), s(2),1) = val;
end
%-----------------------------------------------------------
function out = k(s)
    out = graph(s(1), s(2), 2);
end
function setk(s, val)
    graph(s(1), s(2),2) = val;
end
%-----------------------------------------------------------
function out = calculateKey(x)
    out =  [
        h(x)+g(x)+kM;
%           min( h(x), k(x)+g(x));
          min(h(x), k(x))
        ];
end
function out = testNode(s)
    out = true;
    if template(s(1), s(2)) == 1
        out = false;
        return
    end
    for n = pattern
        pos = s + n;
        if map(pos(1), pos(2)) == 0
            out = false;
            template(s(1), s(2)) = 1;
%             graph(s(1), s(1), 5) = 0;
%             setg(s, -1)
            return
        end
    end
end
function out = cmp(s1, s2)
    out = s1(1) < s2(1) || ( s1(1) ==s2(1) && s1(2) < s2(2));
end
function insert(s, h_new)
        t = inQ(s);
        if t == NEW
            setk(s, h_new);
            seth(s, h_new);
        else
            if t == OPEN
                setk(s, min(k(s), h_new) );
            else
                setk(s, min(h(s), h_new) );
                seth(s, h_new);
            end
            s(3:4)  = [k(s)+kM+g(s); k(s)+g(s)];
            add(stack, s);
            sett(s, OPEN);
        end

end

end


function [Xmat,pobjround,info] = local_search_ape_v1(Zmat,C,rrPar,opt,roundonly)
%% local search for absolute pose estimation
%% used as a subroutine for STRIDE
%% Heng Yang, June 28, 2021

if nargin < 5
    roundonly = false;
end
if nargin < 4
    % default round the first two eigenvectors
    opt = [1,2];
end

tBound        = rrPar.translationBound;
dBound        = rrPar.depthBound;
blk           = rrPar.blk;
FOV           = rrPar.FOV;

if roundonly
    [R,t,theta] = round_ape_v1(Zmat,tBound,dBound,[1]);
    xtld        = lift_ape_v1(R(:),t,theta,tBound,dBound,FOV);
    Xmat        = {xtld{1}*xtld{1}';xtld{2}*xtld{2}';xtld{3}*xtld{3}';xtld{4}*xtld{4}'};
    pobjround   = blktrace(blk,Xmat,C);
else
    [R,t,theta] = round_ape_v1(Zmat,tBound,dBound,opt);
    Zmatround   = {};
    pobjround   = zeros(length(opt),1);
    for i = 1:length(opt)
        Ri              = squeeze(R(:,:,i));
        ti              = squeeze(t(:,i));
        thetai          = squeeze(theta(:,i));
        [out,pobjtmp]   = nlp_ape(C,rrPar,Ri,ti,thetai);
        Zmatround{end+1}   = out.X;
        pobjround(i)       = pobjtmp;
    end
    pobjs            = pobjround;
    [pobjround,idx]  = min(pobjround);
    if pobjround == inf
        fprintf('        NLP fails to find a good solution, return a rounded solution only.\n');
        Ropt         = squeeze(R(:,:,1));
        ropt         = Ropt(:);
        topt         = t(:,1);
        thetaopt     = theta(:,1);
        xopt         = lift_ape_v1(ropt,topt,thetaopt,tBound,dBound,FOV);
        Xmat         = {xopt{1} * xopt{1}';xopt{2} * xopt{2}';xopt{3}*xopt{3}';xopt{4}*xopt{4}'};
        pobjround    = blktrace(blk,Xmat,C);
        nlpsuccess   = false;
    else
        Xmat = Zmatround{idx};
        nlpsuccess   = true;
    end
end

if nargout > 2
    info.minidx     = idx;
    info.nlpsuccess = nlpsuccess;
    info.pobjs      = pobjs;
    info.diffpobj   = pobjs(1) - pobjround;
end

end


function z = aatx(a,x)
%AATX Implicitly compute A * A' * x for sptenmat.
%
%   Z = AATX(A,X) takes a sptenmat object A and computes A * A' *
%   X. This is done without converting A to a standard MATLAB sparse
%   matrix.
%
%   This function is likely most useful as an argument to a routine
%   such as EIGS.
%
%   See also SPTENMAT, SPTENSOR/EIGS.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

subs = a.subs;
s1 = subs(:,1);
s2 = subs(:,2);
m = size(a,1);
n = size(a,2);
vals = a.vals;

v1 = x(s1);
v1 = vals .* v1;
y = accumarray(s2, v1, [n 1]);

v2 = y(s2);
v2 = vals .* v2;
z = accumarray(s1, v2, [m 1]);


% Usage: [xopt, fmin, retcode] = nlopt_minimize_constrained
%                                          (algorithm, f, f_data,
%                                           fc, fc_data, lb, ub,
%                                           xinit, stop)
%
% Minimizes a nonlinear multivariable function f(x, f_data{:}), subject
% to nonlinear constraints described by fc and fc_data (see below), where
% x is a row vector, returning the optimal x found (xopt) along with
% the minimum function value (fmin = f(xopt)) and a return code (retcode).
% A variety of local and global optimization algorithms can be used,
% as specified by the algorithm parameter described below.  lb and ub
% are row vectors giving the upper and lower bounds on x, xinit is
% a row vector giving the initial guess for x, and stop is a struct
% containing termination conditions (see below).
%
% This function is a front-end for the external routine
% nlopt_minimize_constrained in the free NLopt nonlinear-optimization
% library, which is a wrapper around a number of free/open-source
% optimization subroutines.  More details can be found on the NLopt
% web page (ab-initio.mit.edu/nlopt) and also under
% 'man nlopt_minimize_constrained' on Unix.
%
% f should be a handle (@) to a function of the form:
%
%    [val, gradient] = f(x, ...)
%
% where x is a row vector, val is the function value f(x), and gradient
% is a row vector giving the gradient of the function with respect to x.
% The gradient is only used for gradient-based optimization algorithms;
% some of the algorithms (below) are derivative-free and only require
% f to return val (its value).  f can take additional arguments (...)
% which are passed via the argument f_data: f_data is a cell array
% of the additional arguments to pass to f.  (Recall that cell arrays
% are specified by curly brackets { ... }.  For example, pass f_data={}
% for functions that require no additional arguments.)
%
% A few of the algorithms (below) support nonlinear constraints,
% in particular NLOPT_LD_MMA and NLOPT_LN_COBYLA.  These (if any)
% are specified by fc and fc_data.  fc is a cell array of
% function handles, and fc_data is a cell array of cell arrays of the
% corresponding arguments.  Both must have the same length m, the
% number of nonlinear constraints.  That is, fc{i} is a handle
% to a function of the form:
%
%   [val, gradient] = fc(x, ...)
%
% (where the gradient is only used for gradient-based algorithms),
% and the ... arguments are given by fc_data{i}{:}.
%
% If you have no nonlinear constraints, i.e. fc = fc_data = {}, then
% it is equivalent to calling the the nlopt_minimize() function,
% which omits the fc and fc_data arguments.
%
% stop describes the termination criteria, and is a struct with a
% number of optional fields:
%     stop.ftol_rel = fractional tolerance on function value
%     stop.ftol_abs = absolute tolerance on function value
%     stop.xtol_rel = fractional tolerance on x
%     stop.xtol_abs = row vector of absolute tolerances on x components
%     stop.fmin_max = stop when f < fmin_max is found
%     stop.maxeval = maximum number of function evaluations
%     stop.maxtime = maximum run time in seconds
%     stop.verbose = > 0 indicates verbose output
% Minimization stops when any one of these conditions is met; any
% condition that is omitted from stop will be ignored.  WARNING:
% not all algorithms interpret the stopping criteria in exactly the
% same way, and in any case ftol/xtol specify only a crude estimate
% for the accuracy of the minimum function value/x.
%
% The algorithm should be one of the following constants (name and
% interpretation are the same as for the C function).  Names with
% _G*_ are global optimization, and names with _L*_ are local
% optimization.  Names with _*N_ are derivative-free, while names
% with _*D_ are gradient-based algorithms.  Algorithms:
%
% NLOPT_GD_MLSL_LDS, NLOPT_GD_MLSL, NLOPT_GD_STOGO, NLOPT_GD_STOGO_RAND,
% NLOPT_GN_CRS2_LM, NLOPT_GN_DIRECT_L, NLOPT_GN_DIRECT_L_NOSCAL,
% NLOPT_GN_DIRECT_L_RAND, NLOPT_GN_DIRECT_L_RAND_NOSCAL, NLOPT_GN_DIRECT,
% NLOPT_GN_DIRECT_NOSCAL, NLOPT_GN_ISRES, NLOPT_GN_MLSL_LDS, NLOPT_GN_MLSL,
% NLOPT_GN_ORIG_DIRECT_L, NLOPT_GN_ORIG_DIRECT, NLOPT_LD_AUGLAG_EQ,
% NLOPT_LD_AUGLAG, NLOPT_LD_LBFGS, NLOPT_LD_LBFGS_NOCEDAL, NLOPT_LD_MMA,
% NLOPT_LD_TNEWTON, NLOPT_LD_TNEWTON_PRECOND,
% NLOPT_LD_TNEWTON_PRECOND_RESTART, NLOPT_LD_TNEWTON_RESTART,
% NLOPT_LD_VAR1, NLOPT_LD_VAR2, NLOPT_LN_AUGLAG_EQ, NLOPT_LN_AUGLAG,
% NLOPT_LN_BOBYQA, NLOPT_LN_COBYLA, NLOPT_LN_NELDERMEAD,
% NLOPT_LN_NEWUOA_BOUND, NLOPT_LN_NEWUOA, NLOPT_LN_PRAXIS, NLOPT_LN_SBPLX
%
% For more information on individual algorithms, see their individual
% help pages (e.g. "help NLOPT_LN_SBPLX").
function [xopt, fmin, retcode] = nlopt_minimize_constrained(algorithm, f, f_data, fc, fc_data, lb, ub, xinit, stop)

opt = stop;
if (isfield(stop, 'minf_max'))
  opt.stopval = stop.minf_max;
end
opt.algorithm = algorithm;
opt.min_objective = @(x) f(x, f_data{:});
opt.lower_bounds = lb;
opt.upper_bounds = ub;
% opt.local_optimizer = alg;
% for i = 1:length(fc)
%   opt.fc{i} = @(x) fc{i}(x, fc_data{i}{:});
% end
opt.fc = fc;
[xopt, fmin, retcode] = nlopt(opt, xinit);


function r = atan(p)
%UMINUS: this function calculates the atan of a x, y, and z values of
% the structures that I use in the FVtool.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Copyright (c) 2012-2016 Ali Akbar Eftekhari
% See the license file

r = p;
r.xvalue = atan(p.xvalue);
r.yvalue = atan(p.yvalue);
r.zvalue = atan(p.zvalue);


% 3D molecular dynamics (GPU vectorized)
% by Brian Haidet for AlphaPhoenix
% published 12/15/2018
% CC non-commercial, attribution

% Don't copy this straight into a homework or something rediculous - I'm
% posting this so people can hopefully learn from it! It's not the cleanest
% code, but I added a few more comments in the areas that need
% explaining. and deleted all the extra bits of commented-out code that had
% built up. Feel free to ask more questions on youtube here!
% https://youtu.be/6DlRsPo-dxY

function [] = plotter(iteration)
    load(['c:\MatlabOutput\CsClSimFrames\data\data' num2str(iteration,'%05d') '.mat'])
    %load(['H:\MatlabOutput\CsClSimFrames\data32krun\' num2str(iteration,'%05d') '.mat'])

%PLOTTER Summary of this function goes here
%   Detailed explanation goes here
spheresize=20;
[X,Y,Z]=sphere(spheresize-1);
cdb=zeros(spheresize,spheresize,3);
cdb(:,:,3)=ones(spheresize,spheresize)+((Z-1)/2);
cdr=zeros(spheresize,spheresize,3);
cdr(:,:,1)=ones(spheresize,spheresize)+((Z-1)/2);
X=.7*X;
Y=.7*Y;
Z=.7*Z;

gasFraction=.05;
gasFraction2=.0025;
coldThreshold=.06;
annealNum=1024; %above 2048, annealing instead of instachilling
annealMedianSpeed=.022; %cool until this is the median speed of the simulation
speedLimit=.4;

set(gcf,'color','w');
subplot(3,3,[2,3,5,6,8,9]);
hold on;
xlim([-1-1,l+1+1]);
ylim([-1-1,l+1+1]);
zlim([-1-1,l+1+1]);
axis vis3d;
axis off;
zoom;
setAxes3DPanAndZoomStyle(zoom(gcf),gca,'camera');
view(45+iteration*.5,15);
numParticles=size(r,2);

% ---------------------------- calculate which particles to draw -------------------
% thresholdDist=1.25; %between 2nd and 3rd nearest neighbors
% thresholdCount=14; %number of nearest and 2nd nearest neighbors
% rG=gpuArray(r);
% totForces=gpuArray(zeros(1,numParticles));
% chunkSize=ceil(8192^2/numParticles);
% for i=1:ceil(numParticles/chunkSize) %for each chunk
%     cInds=(i-1)*chunkSize+1 : min((i)*chunkSize,numParticles);
%
%     numPcurr=length(cInds);
%     IP=rG(:,cInds)'*rG(:,:);
%     forces=gpuArray([]);
%     distances=(sqrt(abs(bsxfun(@plus,sum(rG(:,cInds)'.^2,2),sum(rG(:,:).^2,1))-2*IP))<thresholdDist);
%     totForces(cInds)=sum(distances,2);
% end
% drawParticles=totForces<thresholdCount;
% drawPartsIndex=find(drawParticles);
% ----------------------------------------------------------------------------------

% for p=drawPartsIndex
for p=1:numParticles
        delta=mean(r(:,1),2)-[l/2;l/2;l/2];
        %saveData(:,:,f) = saveData(:,:,f) -
        if charge(p)==1
            h=surface(X+r(1,p)-delta(1),Y+r(2,p)-delta(2),Z+r(3,p)-delta(3),cdr);
        else
            h=surface(X+r(1,p)-delta(1),Y+r(2,p)-delta(2),Z+r(3,p)-delta(3),cdb);
        end
        set(h,'edgecolor','none');
end
hold off;
% set(gcf, 'Position', [0 0 1920, 1080]); %<- Set size
% %set(gcf, 'renderer', 'opengl');

subplot(3,3,[4])
[counts,centers] = hist(speedsS*10*30,200);
%semilogy(centers, counts, 'ro', 'MarkerFace', 'r');
semilogy(centers, smooth(counts), 'b-');
xlabel('Distribution of Particle Speeds (units/second)');
xlim([0,max(.15*10*30,speedsS(end)*10*30)]);

title(['CsCl Crystal Structure (equal sized ions)' char(10) ...
    char(10)...
    'Computation Time: ' datestr(compTime/86400, 'DD:HH:MM:SS.FFF') char(10) ...%'GPUTemp: ' num2str(GpuT) ' Celcius' char(10) ...
    char(10) ...
    'Simulation Time: ' datestr(iteration/30/86400, 'HH:MM:SS.FFF') char(10) ...
    'Current Step: ' num2str(maxNum) char(10) ...
    'Number of Particles: ' num2str(numParticles) char(10) ...
    char(10) ...
    'Nucleating until 64 particles' char(10) ...
    'Annealed growth past ' num2str(annealNum) ' particles' char(10) ...
    'Currently cooling crystal with ' num2str(ceil(maxNum/4)) ' sink particles.' char(10) ...
    'Heated walls (2% increase in speed per bounce).' char(10) ...
    'Global speed limit of ' num2str(speedLimit*10*30) ' units/second'])

subplot(3,3,7)
plot(speedsS*10*30, 'b.');%arrayfun(@(it) norm(r(:,it)-rl(:,it)),1:numParticles));
hold on
plot([0,numParticles],[coldThreshold*10*30,coldThreshold*10*30],'r-')
plot([numParticles*(1-gasFraction)],[coldThreshold*10*30],'rx')
plot([numParticles*(1-gasFraction2)],[coldThreshold*10*30],'rx')
plot([0,numParticles],[annealMedianSpeed*10*30,annealMedianSpeed*10*30],'c-')
plot([numParticles*(.5)],[annealMedianSpeed*10*30],'cx')
hold off
ylim([0,max(.15*10*30,speedsS(end)*10*30)]);
xlim([0,numParticles]);
xlabel('All particles (sorted by speed)')
ylabel('Particle velocities')
%drawnow();
% saveas(gcf,['H:\MatlabOutput\CsClSimFrames\frames\Frame' num2str(iteration,'%05i') '.png']);
%fighand = gcf;
end


% Authors: A. Iscen, G. Tolias, Y. Avrithis, T. Furon, O. Chum. 2017.
% function to perform diffusion
% solving system A*f = y
function f = dfs(A, y, tol, it)
	if nargin < 4, it = 20;	end
	if nargin < 3, tol = 1e-10; end

   [f,~,~,~] = pcg(A,y,tol,it);


classdef (InferiorClasses = {?rotation,?quaternion}) SO3Grid < orientation
% represent orientations in a gridded structure to allow quick access
%
% Syntax
%   S3G = regularSO3Grid(CS)
%   S3G = regularSO3Grid(CS,SS,'resolution',2.5*degree)     % specify the resolution
%   S3G = regularSO3Grid(CS,SS,'resolution',5*degree,'ZYZ') % use ZYZ convention
%   S3G = regularSO3Grid(CS,SS,'phi2','sections',10) % 10 phi2 sections
%   S3G = regularSO3Grid
%   S3G = equispacedSO3Grid(CS,SS,'points',n)
%   S3G = equispacedSO3Grid(CS,'resolution',res)
%
%   % fill only a ball with radius of 20 degree
%   S3G = equispacedSO3Grid(CS,'maxAngle',20*degree)
%
% Input
%  CS  - @crystalSymmetry
%  SS  - @specimenSymmetry
%   n  - approximate number of points
%  res - resolution in radiant
%
% Output
%  S3G - @SO3Grid
%
% Options
%  maxAngle - radius of the ball to be filles
%  center - center of the ball
%  sections - number of sections
%
% Flags
%  phi1 | Phi | phi2 - sections along which variable
%

  properties
    alphabeta = [];
    gamma    = [];
    resolution = 2*pi;
    center  = [];
  end

  methods
    function S3G = SO3Grid(ori,alphabeta,gamma,varargin)

      % call superclass method
      S3G = S3G@orientation(ori);

      S3G.alphabeta = alphabeta;
      S3G.gamma = gamma;
      S3G.center = get_option(varargin,'center',quaternion.id);
      S3G.resolution = get_option(varargin,'resolution',2*pi);
    end
  end
end


%--------------------------------------------------------------------------------------------------------
% The system is created based on the principles described in the following paper
% Jimmy SJ. Ren, Li Xu, Qiong Yan, Wenxiu Sun, "Shepard Convolutional Neural Networks",
% Advances in Neural Information Processing Systems (NIPS 2015)
% email: jimmy.sj.ren@gmail.com
%--------------------------------------------------------------------------------------------------------
addpath applications/Shepard_CNN/Shepard_super_res/
addpath applications/Shepard_CNN/utility/
addpath applications/image_denoise/utility/
addpath utils/
addpath cuda/
addpath mem/
addpath layers/
addpath layers_adapters/
addpath pipeline/

clearvars -global config;
clearvars -global mem;
clear gen_mask_patch_cat_idx_for_super_res;
clear;
global config;

% set5
% -- baby bird butterfly head woman
I = im2double(imread('applications/Shepard_CNN/Shepard_super_res/images/Set5/butterfly_GT.bmp'));
% set 14
% -- baboon barbara bridge coastguard comic face flowers foreman lenna man monarch pepper ppt3 zebra
I = im2double(imread('applications/Shepard_CNN/Shepard_super_res/images/Set14/ppt3.bmp'));
if(size(I,3) == 3)
    I = rgb2ycbcr(I);
end

I = modcrop(I, 4);
I = padarray(I, [4 4], 'replicate');

% downsample the image
D3chs = imresize(I, [size(I,1)/2 size(I,2)/2], 'bicubic');
D = D3chs(:,:,1);

% super-resolution x2
U = imresize(D, [size(I,1) size(I,2)], 'nearest');
prepare_sr_net(size(U, 1), size(U, 2), 'applications/Shepard_CNN/Shepard_super_res/results/shepard_layer_x2/sr_x2.mat');
mask = config.NEW_MEM([1 0; ...
                       0 0]);
mask = repmat(mask, size(U, 1)/2, size(U, 2)/2, 1);

final_output = apply_net_with_mask(config.NEW_MEM(U) .* mask, mask);
final_output = gather(final_output);
D3chs = imresize(D3chs, [size(final_output,1),size(final_output,2)]);
final_output = shave(final_output, [2 2]);
D3chs = shave(D3chs, [2 2]);

final_output = cat(3, final_output, D3chs(:,:,2:3));
final_output = ycbcr2rgb(double(final_output));
I_bicubic = ycbcr2rgb(D3chs);

figure('name', 'bicubic'), imshow(I_bicubic);
figure('name', 'Sheperd CNN'), imshow(final_output);
drawnow();


function model = standard_classifier_train(X,Y,T,options,binsize)
% Fit a classifier to the data given in X with labels Y of the type
% specified in the options - see below for details
%
% INPUT
% X: Brain data, (total time by regions) or (time by trials by regions)
% Y: Stimulus, (total time by q). This variable can either be entered as a
%       single vector of class assignments (ie q=1 and y takes values
%       1,2,...N=total number of classes); or can be entered as a matrix of
%       labels where each column contains 1 or 0 denoting whether that
%       class is active.
% T: Length of series
%       options             structure with the following subfields:
%           classifier      String indicating calssifer type;
%                   logistic    Logistic Regression classifier (default)
%                   SVM         Linear support vector machine classifier
%                   LDA         Linear Discriminant Analysis classifier
%           regularisation  String indicating type of regularisation;
%                   L1          Lasso regularisation
%                   L2          Ridge regularisation (default)
%                   ARD         Automatic relevance determination
%           lambda          Vector of regularisation penalty values to be
%                           tested
% binsize: NOT YET IMPLMENTED
%           how many consecutive time points will be used for each estiamtion.
%           By default, this is 1, such that one decoding model
%               is estimated using 1 time point
%
% OUTPUT
%
%
% Author: Cam Higgins, OHBA, University of Oxford (2019)

if nargin < 4 || isempty(options), options = struct(); end
if nargin < 5, binsize = 1; end

if ~all(T==T(1)), error('All elements of T must be equal for time-aligned classification'); end
N = length(T); ttrial = T(1);

if size(Y,1) < size(Y,2); Y = Y'; end
q = size(Y,2);
if size(Y,1) == length(T) % one value per trial
    responses = Y;
    Ystar = Y;
elseif length(Y(:)) ~= (ttrial*N*q)
    error('Incorrect dimensions in Y')
else
    responses = reshape(Y,[ttrial N q]);
    Ystar = reshape(repmat(responses(1,:,:),[ttrial,1,1]),[ttrial*N q]);
    responses = permute(responses(1,:,:),[2 3 1]); % N x q
    if ~all(Y(:)==Ystar(:))
        error('For time-aligned classification, the same stimulus must be presented for the entire trial');
    end
end

Ycopy = Y;
if size(Ycopy,1) == N
    Ycopy = repmat(reshape(Ycopy,[1 N q]),[ttrial 1 1]);
else
    Ycopy = reshape(Ycopy,[ttrial N q]);
end
% no demeaning by default if this is a classification problem
if ~isfield(options,'demeanstim'), options.demeanstim = 0; end

options.Nfeatures = 0;
options.K = 1;
[X,Y,T] = preproc4hmm(X,Y,T,options);
Q_star = size(Y,2); % for multinomial and LDA (mean inserted) models

p = size(X,2);

X = reshape(X,[ttrial N p]);
Y = reshape(Y,[ttrial N Q_star]);

if (binsize/2)==0
    warning(['binsize must be an odd number, setting to ' num2str(binsize+1)])
    binsize = binsize + 1;
end

if any(T~=ttrial), error('All trials must have the same length'); end
if nargin<5, binsize = 1; end

if isfield(options,'classifier')
    classifier = options.classifier; options = rmfield(options,'classifier');
    if ~(strcmp(classifier,'logistic') || strcmp(classifier,'SVM') ||...
            strcmp(classifier,'LDA') || strcmp(classifier,'KNN') || ...
            strcmp(classifier,'decisiontree') || strcmp(classifier,'SVM_rbf'))
        error('options.classifier can only take values "logistic", "SVM" or "LDA"');
    end
else, classifier = 'logistic';
end
model = struct();
model.classifier = classifier;
if isfield(options,'lambda')
    lambda = options.lambda; options = rmfield(options,'lambda');
else, lambda = 0.001;
end
if isfield(options,'regularisation')
    regtype = options.regularisation; options = rmfield(options,'regularisation');
    if ~(strcmp(regtype,'L1') || strcmp(regtype,'L2') || strcmp(regtype,'ARD'));
        error('options.regularisation can only take values "L1", "L2" or "ARD"');
    end
else, regtype = 'L2';
end

halfbin = floor(binsize/2);
%nwin = round(ttrial / binsize);
%binsize = floor(ttrial / nwin);
%RidgePen = lambda * eye(p);

%reshape to 3d vectors:
if strcmp(classifier,'logistic')
    % alternative: model = synchronousLogReg(X,Y,T,options)
    if length(unique(Ystar))<=2
        Y(Y==-1) = 0;
    end
    for t  = 1:ttrial
        if options.verbose
            if strcmp(regtype,'L1')
                fprintf(['\nRunning LASSO Regression analysis on data for t=',int2str(t),'th sample']);
            elseif strcmp(regtype,'L2')
                fprintf(['\nRunning RIDGE Regression analysis on data for t=',int2str(t),'th sample']);
            elseif strcmp(regtype,'ARD')
                error('ARD regularisation not implemented yet for logistic regression');
            end
        end
        for ilambda = 1:length(lambda)
            betas = zeros(p,Q_star);
            intercepts = zeros(1,Q_star);
            for c = 1:q
                if length(unique(Ystar))>2
                    vp = Y(t,:,c)~=0;
                    Ysample = Y(t,vp,c)'==1;
                else
                    vp = true(1,N);
                    Ysample = Y(t,vp,c)';
                end
                if strcmp(regtype,'L1')
                    [betas(:,c),fitInfo] = lassoglm(squeeze(X(t,vp,:)),Ysample, ...
                        'binomial','Alpha', 1, 'Lambda', lambda(ilambda),'Standardize', false);
                    intercepts(c) = fitInfo.Intercept;
                elseif strcmp(regtype,'L2')
                    if p>1
                        [betas(:,c),fitInfo] = lassoglm(squeeze(X(t,vp,:)),Ysample, ...
                            'binomial','Alpha', 0.00001, 'Lambda', lambda(ilambda),'Standardize', false);
                    else
                        [betas(:,c),fitInfo] = lassoglm(squeeze(X(t,vp,:))',Ysample, ...
                            'binomial','Alpha', 0.00001, 'Lambda', lambda(ilambda),'Standardize', false);
                    end
                    intercepts(c) = fitInfo.Intercept;
                end
            end
            model.betas(t,:,:,ilambda) = betas;
            model.intercepts(t,:,ilambda) = intercepts;
        end
        model.lambda = lambda;
    end
elseif strcmp(classifier,'SVM')
    model.SVM=cell(ttrial,Q_star);
    for t=1:ttrial
        if options.verbose;
            fprintf(['Inferring SVM (linear) classifiers for t = ',int2str(t),'th sample\n']);
        end
        for iStim=1:Q_star
            model.SVM{t,iStim} = fitclinear(squeeze(X(t,:,:)),squeeze(Y(t,:,iStim)));%,ones(sum(tselect(:,t))));
        end
    end
elseif strcmp(classifier,'SVM_rbf')
    model.SVM=cell(ttrial,Q_star);
    for t=1:ttrial
        if options.verbose;
            fprintf(['Inferring SVM (w radial basis function) classifiers for t = ',int2str(t),'th sample\n']);
        end
        for iStim=1:Q_star
            model.SVM{t,iStim} = fitcsvm(squeeze(X(t,:,:)),squeeze(Y(t,:,iStim)));%,ones(sum(tselect(:,t))));
        end
    end
elseif strcmp(classifier,'LDA')
    if isfield(options,'CVmethod')
        options_LDA = rmfield(options,'CVmethod');
    elseif isfield(options,'accuracyType')
        options_LDA = rmfield(options,'accuracyType');
    end
    options_LDA.useUnsupervisedGamma = 1; %this ensures gamma is fixed for standard decoding
    options_LDA.verbose = 1;
    options_LDA.useParallel = 0;
    options_LDA.standardise = 0;
    options_LDA.K = ttrial;
    options_LDA.Gamma = repmat(eye(ttrial),length(T),1);
    if isfield(options,'covtype');
        options_LDA.covtype = options.covtype;
    else
        options_LDA.covtype = 'full';
    end
    X_LDA = reshape(X,[ttrial*N p]);
    Y_LDA = reshape(Y,[ttrial*N Q_star]);
    if options.verbose
        fprintf(['Inferring LDA classifiers for all timepoints \n']);
    end
    [model,Gamma] = encodertrain(X_LDA,Y_LDA,T,options_LDA);
    model.Gamma = Gamma(1:ttrial,:);
    %model=rmfield(model,{'prior','Dir_alpha','Dir2d_alpha','P','Pi','features','K'});
    model.classifier='LDA';
elseif strcmp(classifier,'KNN')
    model.X_train = X;
    model.Y_train = Y;
elseif strcmp(classifier,'decisiontree')
    for t=1:ttrial
        Y_temp=zeros(N,1);
        for n=1:N;Y_temp(n) = find(squeeze(Y(t,n,:)));end
        model.decisiontree{t} = fitctree(squeeze(X(t,:,:)),Y_temp);
    end
end

end


 function xs = tpl_inc(x, Gb, yi, bi, ri, R, niter, pixmax, curv, ...
		subout, enhance, gi, denom, relax0, chat)
%function xs = tpl_inc(x, Gb, yi, bi, ri, R, niter, pixmax, curv, ...
%		subout, enhance, gi, denom, relax0, chat)
%
% TRIOT: incremental SPS algorithm for transmission Poisson problem
% (ordered subsets separable paraboloidal surrogates)
% in
%	x	[np,1]		initial estimate
%	Gb			Gblock object (see tpl_os_sps_test.m)
%	yi,bi,ri [nb,na]	see tr_fbp.m (for model too)
%	R		penalty structure (see Robject.m)
%				(or empty for ML case)
%	niter		# iterations
%	pixmax		upper constraint for pixel values or [lower, upper]
%			can be scalar (e.g. 'inf') or an array the size of x
%	curv		'oc' for erdogan's optimal curvatures
%			'mc' for maximum curvature
%			'pc' for erdogan's fast precomputed curvatures,
%				which usually provides faster convergence,
%				but can be nonmonotone.
%	subout	[1]	nonzero value: return all subiterates
%			otherwise, return only iterates
%	enhance [1]	nonzero value: Hsiao's enhancement
%			otherwise, no enhancement
%	gi	[nd,1]	sum(G')'
%	denom	[np,1]		(if precomputed denominator)
%	relax0	[1] or [2]	relax0 or (relax0, relax_rate)
% out
%	xs [np,niter]	updated image vectors each iteration
%
% Copyright Mar 2000, Jeff Fessler, The University of Michigan
% 2002-2-20 update for Gblock object
% Modified for incremental version by Sangtae Ahn, from ~sangtaea/trans.

epsilon = 1e-10;

if nargin < 3, ir_usage, end

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

if ~isvar('bi') || isempty(bi)
	bi = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

if ~isvar('R'), R = []; end
if ~isvar('niter')	|| isempty(niter),	niter = 2;	end
if ~isvar('pixmax')	|| isempty(pixmax),	pixmax = inf;	end
if length(pixmax) == 2
	pixmin = pixmax(1);
	pixmax = pixmax(2);
else
	pixmin = 0;
end
if ~isvar('curv')	|| isempty(curv),	curv = 'oc';	end
if ~isvar('subout')	|| isempty(subout),	subout = 0;	end
if ~isvar('enhance')	|| isempty(enhance),	enhance = 0;	end
if ~isvar('chat')	|| isempty(chat),	chat = false;	end
if ~isvar('relax0')	|| isempty(relax0),	relax0 = 1;	end

if streq(curv, 'mc')
	subscale = @(iset) iset;
else
	subscale = @(iset) 1;
end

if length(relax0) == 1
	relax_rate = 0;
elseif length(relax0) == 2
	relax_rate = relax0(2);
	relax0 = relax0(1);
else
	error relax
end

if length(niter) == 2
	osspsiter = niter(1);
	niter = niter(2);
else
	osspsiter = 1;
end

trl_check(yi, bi, ri);

if ~isvar('gi') || isempty(gi)
	gi = reshape(sum(Gb'), size(yi));	% g_i = sum_j g_ij
end

starts = subset_start(nblock);
[nb na] = size(yi);

% precompute denominator if needed
if (~isvar('denom') || isempty(denom)) && (streq(curv, 'pc') || streq(curv, 'mc'))
	ni = my_trl_curvature(yi, bi, ri, [], curv); % precomputed curvatures
	%
	% separate denominators for each subset
	%
	if streq(curv, 'mc')
		denom = zeros(numel(x), nblock);
		for iset=1:nblock
			iblock = starts(iset);
			ia = iblock:nblock:na;
			denom(:,iset) = Gb{iblock}' * col(gi(:,ia) .* ni(:,ia));
		end
	else
	%
	% one denominator shared by all subsets
	%
		denom = Gb' * col(gi .* ni) / nblock;
	end
end

% "precomputed curvature"
if ~streq(curv, 'pc')
	ni = my_trl_curvature(yi, bi, ri, [], 'pc');
	denom_pc = Gb' * col(gi .* ni) / nblock;
else
	denom_pc = denom;
end

if subout
	xs = zeros(length(x), (niter-1)*nblock+1);
else
	xs = zeros(length(x), niter);
end

x = max(x,pixmin);
x = min(x,pixmax);
xs(:,1) = x;
if subout, idx = 2; end
num_store = zeros(length(x), nblock);
den_store = zeros(length(x), nblock);

%
% initialization by os-sps-pc
%
for iter = 1:osspsiter
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Gb{iblock} * x;	% l=G*x "line integrals"
		li = reshape(li, nb, length(ia));
		bel = bi(:,ia) .* exp(-li);
		yb = bel + ri(:,ia);	% predicted meas. means
		dothi = (1 - yi(:,ia) ./ yb) .* bel;

		if streq(curv, 'oc')
			ni = my_trl_curvature(yi(:,ia), bi(:,ia), ri(:,ia), ...
				li, 'oc');
			denom = Gb{iblock}' * col(gi(:,ia) .* ni);
		end

		if isempty(R)
			grad = Gb{iblock}' * dothi(:);
			den_osps = max(denom_pc, epsilon);
			den_store(:, iset) = max(denom(:,subscale(iset)), ...
						epsilon);
		else
			grad = Gb{iblock}' * dothi(:) - R.cgrad(R, x)/nblock;
			den_osps = max(denom_pc + R.denom(R, x)/nblock, ...
					epsilon);
			den_store(:, iset) = max(denom(:,subscale(iset)) + ...
					R.denom(R, x)/nblock, epsilon);
		end

		if iter == osspsiter
			num_store(:, iset) = x.*den_store(:, iset) + grad;
		end
		x = x + grad ./ den_osps;
		x = max(x,pixmin);
		x = min(x,pixmax);
		if subout
			xs(:,idx) = x;
			idx = idx + 1;
		end
	end
	if ~subout, xs(:,iter+1) = x;	end
end
num = sum(num_store, 2);
den = sum(den_store, 2);

if 1,		% optional
	x = num ./ den;
	x = max(x,pixmin); x = min(x,pixmax);
	if subout
		xs(:,idx-1) = x;
	else
		xs(:,iter+1) = x;
	end
end

alphidx = 1;
for iter = (osspsiter+2):niter
	ticker(mfilename, iter, niter)

	relax = relax0 / (1 + relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Gb{iblock} * x;	% l=G*x "line integrals"
		li = reshape(li, nb, length(ia));
		bel = bi(:,ia) .* exp(-li);
		yb = bel + ri(:,ia);	% predicted meas. means
		dothi = (1 - yi(:,ia) ./ yb) .* bel;

		% optimal curvatures (for ensured monotone increase)
		if streq(curv, 'oc')
			ni = my_trl_curvature(yi(:,ia), bi(:,ia), ri(:,ia), ...
				li, 'oc');
			denom = Gb{iblock}' * col(gi(:,ia) .* ni);
		end

		num = num - num_store(:, iset);
		den = den - den_store(:, iset);

		if isempty(R)
			grad = Gb{iblock}' * dothi(:);
			den_store(:, iset) = max(denom(:,subscale(iset)), ...
						epsilon);
		else
			grad = Gb{iblock}' * dothi(:) - R.cgrad(R, x)/nblock;
			den_store(:, iset) = max(denom(:,subscale(iset)) + ...
						R.denom(R, x)/nblock, epsilon);
		end

		num_store(:, iset) = x.*den_store(:, iset) + grad;

		num = num + num_store(:, iset);
		den = den + den_store(:, iset);

		xold = x;

		x = num ./ den;
		x = max(x,pixmin); x = min(x,pixmax);

		if enhance
			xos = xold + grad ./ (den_store(:,iset) - ...
				denom(:,subscale(iset)) + denom_pc);
			xos = max(xos,pixmin);
			xos = min(xos,pixmax);

			c2 = - sum((xos - x).^2 .* den) / 2;
			c1 = num'*(xos - x) - sum(x.*(xos-x).*den);
			c0 = num'*(x - xold) - sum(x.^2.*den - xold.^2.*den)/2;

			if 1
				if c0+c1+c2 > 0
					alpha = 1;
				else
					alpha = eql_root(-c2, -c1/2, 0.95*c0);
					alpha = min(1, alpha);
					alpha = max(0, alpha);
				end
			else
				alpha = 1/0.9;
				del_obj = -999;
				while (del_obj <=0) & (alpha > 0.0096)
					alpha = alpha * 0.9;
					del_obj = c2*alpha^2 + c1*alpha + c0;
				end

				if alpha <= 0.0096, alpha = 0; end
			end

			x = (1-alpha)*x + alpha*xos;
			alph(alphidx) = alpha;
			alphidx = alphidx + 1;

		end

		if subout
			xs(:,idx) = x;
			idx = idx + 1;
		end
	end

	if chat, printf('Range %g %g', min(x), max(x)), end
	if ~subout, xs(:,iter) = x; end
end

%figure, plot(alph, 'o-')

function obj = trl_obj(alpha, xinc, xos, linc, los, yi, bi, ri, R)

x = alpha*xos + (1 - alpha)*xinc;
li = alpha*los + (1 - alpha)*linc;
yp = bi .* exp(-li) + ri;
like = sum(yi .* log(yp) - yp);
penal = sum(R.penal(R, x));
obj = like - penal;
obj = -obj;


function [OptExchRxns] = findOptExchRxns(model,Ex_Rxns,parFVA)
% USAGE:
%
%    [OptExchRxns] = findOptExchRxns(model, Ex_Rxns, parFVA)
%
% INPUTS:
%    model:         Metabolic model
%    Ex_Rxns:       Vector of exchange reactions for FVA
%    fastFVA:       use FastFVA (default=0)
%
% OUTPUTS:
%    OptExchRxns:
%
% .. Authors:
%       - Ines Thiele 2014
%       - Maike K. Aurich 27/05/15, remove hidden values

tol = -1e-6;  % Default tolerance

if ~exist('parFVA','var') || parFVA == 0

    matlabpool = 0;
    parFVA = 0; % default no parallelization of FVA
end

[a1(:,1),a1(:,2)]=fluxVariability(model,1,[],Ex_Rxns);
% find all uptakes that have ub=0;
if size(a1,1)==length(Ex_Rxns)
    OptSecr = Ex_Rxns;
    R = OptSecr(find(a1(:,2)<abs(tol)));
    R = [R; OptSecr(find(a1(:,1)>abs(tol)))];
    OptSecr(ismember(OptSecr, R))=[];% remove obligate uptake
    clear R
    OptUptake = Ex_Rxns;
    R = OptUptake(find(a1(:,2)<(tol)));
    R = [R; OptUptake(find(a1(:,1)>(tol)))];
    OptUptake(ismember(OptUptake, R))=[];% remove obligate uptake
    OptExchRxns = [OptUptake;OptSecr];
else
    OptExchRxns=[];
end


% [template t2 valid] = PPG_SQI(wave,anntime,annot,template_ahead,windowlen,Fs)
%
% PPG_SQI.m - PPG SQI based on beat template correlation.
% (as an advice, the algorithm get 30 beats at each call and run in loop)
% by Qiao Li 30 Mar 2011
%
% PPG sampling frequency is Fs
%
% input:
%     wave:       PPG data;
%     anntime:    PPG annotation time (samples), read from ple annot file,
%                 But the ann-time is the OFFSET based on wave(1)
%     annot:      Annotation of beats, read from from ple annot file
%                 directly
%     template:   Last PPG beat template
%     windowlen:  length of window to calculate template(default: 30s)
%     Fs       :  sampling frequency (defatult: 125 to work with pervious code)
% output:
%     annot:      ppg sqi annotation
%                     annot.typeMnemonic: E - excellent beat;
%                                         A - acceptable beat;
%                                         Q - unacceptable beat
%                     annot.subtype: SQI based on Direct compare
%                     annot.chan:    SQI based on Linear resampling
%                     annot.num:     SQI based on Dynamic time warping
%                     annot.auxInfo: SQI based on Clipping detection
%     template:   Current PPG beat template
%     valid:      1 or greater for valid template,
%                 0 for invalid template
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%
% 12-01-2107 Modified by Giulia Da Poian: replace fixed samplig frequency
% (125) with a variable Fs

function [annot template valid] = PPG_SQI(wave,anntime,annot,template,windowlen,Fs)

    if nargin < 6
        Fs =125;
    end
    if nargin < 5
        windowlen=30*Fs;
    end
    if nargin < 4
        template=[];
    end
    if nargin < 3
        sprintf('Error: must provide wave, anntime and annot');
        annot=[];
        template=[];
        valid=0;
        return;
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 19/09/2011 ADD baseline wander filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    wave = PPGmedianfilter(wave, Fs,Fs);
    % get PPG template
    [t t2 v]=template_pleth(wave(1:min(windowlen,length(wave))), anntime(find(anntime<min(windowlen,length(wave)))), 0,Fs);

    if v<1 && length(template)<1 % Current template invalid && no previous template available
        for j=1:length(annot)
            annot(j).typeMnemonic='Q'; % Unacceptable
        end
        t=[];
    else
        % Using previous template as the template
        if v<1
            t=template;
        end
        % Using t2 if available
        if v>1
            t=t2;
        end

            % Calculate the PLA of template for dynamic time warping
            d1=t;
            d1=(d1-min(d1))/(max(d1)-min(d1)).*100;
            [y1 pla1]=PLA(d1,1,1);

% Main Loop
            for j=1:length(annot)-1

%             SQI1: Direct compare

%             Calculate correlation coefficients based on the template
%             length
                beatbegin=anntime(j);
                beatend=anntime(j+1);
% 07/11/2011 ADD max beat length <= 3s detection
		if beatend-beatbegin>3*Fs
			beatend=beatbegin+3*Fs;
		end
		templatelength=length(t);
		complength=min(templatelength,beatend-beatbegin-1);
                if beatbegin+complength-1 > length(wave) || beatend > length(wave) || beatbegin < 1
                    continue;
                end
                currentb=j;
                cc=corrcoef(t(1:complength),wave(beatbegin:beatbegin+complength-1));
                c1(j)=cc(1,2);
                if (c1(j)<0)
                    c1(j)=0;
                end
                annot(currentb).subtype=int8(c1(j)*100);

%             SQI2: Linear resampling

%             Calculate correlation coefficients based on the linear
%             resampling (interp1)

                y=interp1(1:beatend-beatbegin, wave(beatbegin:beatend-1),1:(beatend-beatbegin-1)/(templatelength-1):(beatend-beatbegin),'spline');
                y(isnan(y))=0;
                cc=corrcoef(t,y);
                c2(j)=cc(1,2);
                if (c2(j)<0)
                    c2(j)=0;
                end
                annot(currentb).chan=int8(c2(j)*100);

%             SQI3: Dynamic Time Warping

%             Calculate correlation coefficients based on the dynamic time
%             warping

                d2=wave(beatbegin:beatend-1);

                % if beat too long, set SQI=0;
                if (length(d2)>length(d1)*10)
                    c3(j)=0;
                else
                    d2=(d2-min(d2))/(max(d2)-min(d2)).*100;
                   [y2 pla2]=PLA(d2,1,1);

                   [w ta tb] = simmx_dtw(y1,pla1,y2,pla2);
                   [p,q,Dm] = dp_dwt2(w,ta,tb);
                   [ym1 ym2 yout1]=draw_dtw(y1,pla1,p,y2,pla2,q);
                    cc=corrcoef(y1,ym2);
                    c3(j)=cc(1,2);
                    if (c3(j)<0)
                        c3(j)=0;
                    end
                end
                annot(currentb).num=int8(c3(j)*100);

%             SQI4: Clipping detection

                d2=wave(beatbegin:beatend-1);
                y=diff(d2);
                clipthreshold=0.5;
                c4(j)=int8(length(find(abs(y)>clipthreshold))/length(y)*100);

%             SQI: Combined

                sqibuf=[annot(currentb).subtype annot(currentb).chan annot(currentb).num c4(j)];
                if min(sqibuf)>=90
                    annot(currentb).typeMnemonic='E'; % Excellent
                else
                    if length(find(sqibuf>=90))>=3 || (median(sqibuf(1:3))>=80 && sqibuf(1)>=50 && sqibuf(4)>=70) || min(sqibuf)>=70
                        annot(currentb).typeMnemonic='A'; % Acceptable
                    else
                        annot(currentb).typeMnemonic='Q'; % Unacceptable
                    end
                end
                annot(currentb).auxInfo=num2str(int8(c4(j)));

            end

    end
    template=t;
    valid=v;


function [gx,dgdx,dgdp] = g_HRF3(Xt,P,ut,in)
% T2* contrast observation function for HRF Balloon model (log space)
% function [gx,dgdx,dgdp] = g_HRF3(Xt,P,ut,in)
% This function evaluates the hemodynamic static observation equation
% function. HEMODYNAMIC STATES ARE IN LOG SPACE.

n = size(Xt,1);

% Get parameters and states indices

try; TE = in.TE; catch, TE = 0.04; end

if isfield(in,'fullDCM') && in.fullDCM
    % estimated region-specific resting oxygen extraction fractions
    % NB: if P(in.ind1) = 0, E0 = 0.34.
%     [E0,dsdp] = sigm(P(in.ind1)-0.6633,struct('beta',1,'G0',1,'INV',0));
%     E0 = E0(:);
    E0 = 1./(1+exp(-(P(in.ind1)-0.6633)));
    dsdp = E0.*(1-E0);
    % estimated region-specific ratios of intra- to extravascular
    % components of the gradient echo signal (prior mean = 1, log-normally
    % distributed scaling factor)
    epsilon   = exp(P(in.ind2));
    % hemodynamic states indices:
    n1 = in.n1;
    n2 = in.n2;
    n3 = in.n3;
    n4 = in.n4;
    try
        n5 = in.n5;
        nreg = n./5;
    catch
        n5 = [];
        nreg = n./4;
    end
    if isfield(in,'homogeneous') && in.homogeneous
        E0 = E0.*ones(nreg,1);
        dsdp = dsdp.*ones(nreg,1);
        epsilon = epsilon.*ones(nreg,1);
        in.ind1 = [in.ind1:2:2*nreg];
        in.ind2 = [in.ind2:2:2*nreg];
    end
else
    [E0,dsdp] = VBA_sigmoid(P(1)-0.6633);
    E0 = E0(:);
    try
        epsilon = exp(P(2));
        p2 = 1;
    catch
        epsilon = 1;
        p2 = 0;
    end
    % hemodynamic states indices:
    n1 = 1;
    n2 = 2;
    n3 = 3;
    n4 = 4;
    nreg = n./4;
end

%--------------------------------------------------------------------------
% coefficients in BOLD signal model,...
V0 = 4; % resting venous volume
r0 = 25; % intravascular relaxation rate
nu0 = 40.3; % frequency offset
k1 = 4.3.*nu0.*E0.*TE;
k2 = epsilon.*r0.*E0.*TE;
k3 = 1 - epsilon;

% states ...
x3 = exp(Xt(n3,:));         % blood volume v(t)
x4 = exp(Xt(n4,:));         % dHb content q(t)

% ... and derivatives
dgdx = zeros(n,nreg);
dgdp = zeros(size(P,1),nreg);

% Evaluate observation function
x4x3 = exp(log(x4)-log(x3));
gx = V0.*(k1.*(1-x4) + k2.*(1-x4x3) + k3.*(1-x3));

% Evaluate gradient wrt states and parameters
for i=1:nreg
    if isfield(in,'fullDCM') && in.fullDCM
        if isempty(n5)
            dgdx(n1(i):n4(i),i) = [...
                0; ...
                0; ...
                -k3(i).*x3(i)+k2(i).*x4x3(i); ...
                -k1(i).*x4(i)-k2(i).*x4x3(i)].*V0;
        else
            dgdx(n1(i):n5(i),i) = [...
                0; ...
                0; ...
                -k3(i).*x3(i)+k2(i).*x4x3(i); ...
                -k1(i).*x4(i)-k2(i).*x4x3(i); 0].*V0;
        end
        dgdp(in.ind1(i),i) = V0.*4.3.*nu0.*TE.*(1-x4(i)).*dsdp(i) + ...
            V0.*epsilon(i).*r0.*TE.*(1-x4x3(i)).*dsdp(i);
        dgdp(in.ind2(i),i) = V0.*k2(i).*(1-x4x3(i)) - ...
            V0.*epsilon(i).*(1-x3(i));
    else
        dgdx(n1(i):n4(i),i) = [0; 0; ...
            -k3(i).*x3(i)+k2(i).*x4x3(i); ...
            -k1(i).*x4(i)-k2(i).*x4x3(i)].*V0;
        dgdp = V0.*4.3.*nu0.*TE.*(1-x4(i)).*dsdp(i) + ...
            V0.*epsilon(i).*r0.*TE.*(1-x4x3(i)).*dsdp(i);
        if p2
            dgdp(2) = V0.*k2(i).*(1-x4x3(i)) - ...
                V0.*epsilon(i).*(1-x3(i));
            dgdp = dgdp(:);
        end
    end
end

if isfield(in,'homogeneous') && in.homogeneous
    dgdp0 = dgdp;
    dgdp = zeros(2,nreg);
    dgdp(1,:) = sum(dgdp0(in.ind1,:),1);
    dgdp(2,:) = sum(dgdp0(in.ind2,:),1);
end

if isfield(in,'confounds') && ~isempty(in.confounds.X0)
    tsample = ut(in.confounds.indt);
    X0 = kron(eye(nreg),in.confounds.X0(tsample,:));
    gx = gx + X0*P(in.confounds.indp);
    dgdp(in.confounds.indp,:) = X0';
end


% UNDER CONSTRUCTION...
%
%
% Author: Javier Lopez-Calderon

function  d = emg2pulse(ch1, fs)

d=[];

if nargin<1
        help emg2pulse
        return
end
if nargin>3
        error('ERPLAB says: emg2pulse() works until 3 inputs')
end
if size(ch1,1)>1 && size(ch1,2)>1
        error('ERPLAB says: emg2pulse() works with vector inputs')
end

d = abs(ch1);
[b,a] = butter(2,30/fs);

for t=1:size(d,3)
        d(d(1,:,t)<6) = 0;
        d(1,:,t) = filtfilt(b,a, d(1,:,t));
        d(1,:,t) = 100*d(1,:,t);
        m = mean(d(1,:,t));
        sdd = std(d(1,:,t));
        th = m+2*sdd;
        d(d(1,:,t)<th) = 0;
        d(d(1,:,t)>=th) = 100;
end


% run this script AFTER using talairach space utility (tsu.m) to get clusters
% this copies the orignial tsu stuff in ihb_TalSpaceView3D.m, but in the workspace

%------------------------------------------------------------------------------
% Get cluster information
%------------------------------------------------------------------------------
clusters = getappdata(hFigMain, 'clusters');
indClusterToView = getappdata(hFigMain, 'indClusterToView');
cl = clusters(indClusterToView);
iv = cl.hThreshold;

%------------------------------------------------------------------------------
% Figure does not exist - create new one
%------------------------------------------------------------------------------
	hFig3D = figure(...
		'Visible', 'off',...
		'NumberTitle', 'off',...
		'MenuBar', 'none',...
		'Name', '3D view of cluster',...
        'Renderer', ihbdfl_renderer,...
		'Tag', 'ihb_TalSpace3D_fig'...
		);

	%------------------------------------------------------------------------------
    % Create and/or adjust light position
	%------------------------------------------------------------------------------
    hLight = findobj('Type', 'light', 'Tag', 'ihb_lightTalSpace3D');
    cameraPos = get(hAxis3D, 'CameraPosition');
    if isempty(hLight)
	    hLight = light('Tag', 'ihb_lightTalSpace3D', 'Position', cameraPos);
    else
        set(hLight, 'Position', cameraPos);
    end
	%------------------------------------------------------------------------------
    % set callback to light follow the camera
	%------------------------------------------------------------------------------
	set(hFig3D, 'WindowButtonUpFcn', 'ihb_View3DButtonUpFcn');
	view([-150,32]);    % set initial camera position
	lighting phong;     % set lighting algorithm
	rotate3d on;        % enable 3D rotation

    %------------------------------------------------------------------------------
    % Smooth volume data
	%------------------------------------------------------------------------------
    if ihbdfl_bApplySmoothing == 1
        switch ihbdfl_smooth_filter
        case 'box'
            V = smooth3(cl.vTal, 'box', ihbdfl_smooth_size);
        case 'gaussian'
            V = smooth3(cl.vTal, 'gaussian', ihbdfl_smooth_size, ihbdfl_smooth_sd');
        end
    else
	    V = cl.vTal;
    end

    %------------------------------------------------------------------------------
    % Prepare volume data and create isosurface patch object
	%------------------------------------------------------------------------------
    % V is the volume of the cluster.

    FV = isosurface(cl.xTal, cl.yTal, cl.zTal, V, cl.hThreshold);
	hPatch = patch(FV);
	isonormals(cl.xTal, cl.yTal, cl.zTal, cl.vTal, hPatch);
	set(hPatch, 'Tag', 'ihb_surfaceCluster', 'FaceColor', ihbdfl_color_surface, 'EdgeColor', 'none', 'FaceAlpha', ihbdfl_transparency);
    set(hFig3D, 'Visible', 'on');


function [fit, pairs] = calcGBFit(job,varargin)
% fit between child to child misorientations and job.p2c
%
% Syntax
%
%   [fit, pairs] = job.calcGBFit
%
%   % visualize the result
%   [gB, pairId] = job.grains.boundary.selectByGrainId(pairs);
%   plot(gB, fit(pairId) ./ degree, 'linewidth',2);
%   setColorRange([2,8])
%   mtexColorMap white2black
%
%   % consider only P2C neighbors
%   [fit, p2cPairs] = job.calcGBFit('p2c')
%
%   % consider only C2C neighbors
%   [fit, c2cPairs] = job.calcGBFit('c2c')
%s
% Input
%  job - @parentGrainReconstructor
%
% Output
%  fit   - fit between the grain boundary misorientations and job.p2c
%  pairs - list of grainId of the neighboring grains
%

noOpt = ~check_option(varargin,{'p2c','c2c'});

% parent to child neighbors
if noOpt || check_option(varargin,'p2c')

  % extract all parent to child neighbors
  [pairs, oriParent, oriChild] = getP2CPairs(job, varargin{:});

  % compute the misorientation
  mori = inv(oriChild).*oriParent;

  % compute the fit
  fit = angle(mori, job.p2c);

else

  fit = [];
  pairs = [];

end

if noOpt || check_option(varargin,'c2c')

  % extract all child to child neighbors
  [c2cPairs, oriChild] = getC2CPairs(job, varargin{:});
  pairs = [pairs;c2cPairs];

  % compute the corresponding misorientations
  mori = inv(oriChild(:,1)).*oriChild(:,2);

  % misorientation to c2c variants
  fit = [fit;min(angle_outer(mori, job.c2c),[],2)];

end


filename='Gripping_quad_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.5;
optimality_final = 1e-3;
constr_final =1e-3;

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


function [A, b, Aeq, beq, lb, ub] = snd_l_constraints(k, net_load_data, varargin)

    u0_ref = cell2mat(varargin(2));
    f = 1.2;
    g = 0.8;
    p = 1.4;
    A   = []; %A*u<=b
    b   = [];
    Aeq = [1 1 1]; %A*u = b
    beq = net_load_data(k);

    if k ~= 12
        if u0_ref(1,k) * u0_ref(1,k+1) <= 0
            if u0_ref(1,k)>0
                ub = [u0_ref(1,k)*f];
                lb = [u0_ref(1,k+1)*f];
            else
                ub = [u0_ref(1,k+1)*f];
                lb = [u0_ref(1,k)*f];
            end
        elseif u0_ref(1,k)>0 && u0_ref(1,k+1)>0
            if u0_ref(1,k)>u0_ref(1,k+1)
                ub = [u0_ref(1,k)*f];
                lb = [u0_ref(1,k+1)*g];
            else
                ub = [u0_ref(1,k+1)*f];
                lb = [u0_ref(1,k)*g];
            end
        elseif u0_ref(1,k)<0 && u0_ref(1,k+1)<0
            if u0_ref(1,k)>u0_ref(1,k+1)
                ub = [u0_ref(1,k)*g];
                lb = [u0_ref(1,k+1)*f];
            else
                ub = [u0_ref(1,k+1)*g];
                lb = [u0_ref(1,k)*f];
            end
        end
    else
        if u0_ref(1,k) >=0
            ub = [u0_ref(1,k)*f];
            lb = [u0_ref(1,k)*g];
        else
            ub = [u0_ref(1,k)*g];
            lb = [u0_ref(1,k)*f];
        end

    end

    if u0_ref(2,k)>0 %
        ub  = [ub, u0_ref(2,k)*p,  10];
        lb  = [lb, 0, -10];
    else
        ub  = [ub, 0,  10];
        lb  = [lb, u0_ref(2,k)*p, -10];
    end

end


function test_bug1443

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_rejectcomponent ft_componentanalysis

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

cfg = [];
cfg.method = 'fastica';
cfg.numcomponent = 14; % to make it go fast
cfg.randomseed = 13; % so we get the same output each time
comp = ft_componentanalysis(cfg,data);

assert(ft_senstype(comp,'ctf151'))

cfg = [];
cfg.component = 2; % chosen randomly
rej1 = ft_rejectcomponent(cfg, comp, data);
rej2 = ft_rejectcomponent(cfg, comp);

norm(rej2.grad.tra-rej1.grad.tra)/norm(rej2.grad.tra);
figure; imagesc(rej2.grad.tra - rej1.grad.tra); caxis([-1 1])

load standard_sourcemodel3d10mm
load standard_singleshell

cfg = [];
cfg.sourcemodel = sourcemodel;
cfg.headmodel = vol;

cfg.grad = rej1.grad;
grid1 = ft_prepare_leadfield(cfg, rej1);

cfg.grad = rej2.grad;
grid2 = ft_prepare_leadfield(cfg, rej2);

assert(~isequaln(grid1.leadfield,grid2.leadfield))


function [alpha,b,eta,zeta,kappa] = solve_hkl_square_dual_fast(eta,data,Y,lambda,varargin);

% solve the milasso for a given regularization parameter
n = size(data.X,1);
p =length(data.affinity);
pX = size(data.X,2);
affs = data.affinity;
weights = data.weights;

% optional parameters
mingap = 1e-3;
display = 0;
gapeta = 1e-3;

% READ OPTIONAL PARAMETERS
args = varargin;
nargs = length(args);
for i=1:2:nargs
    switch args{i},
        case 'mingap',        mingap = args{i+1};
        case 'gapeta',        gapeta = args{i+1};
        case 'display',        display = args{i+1};
    end
end

if isempty(eta)
    eta = 1/p ./ weights.^2;
end
x = eta .* weights.^2;
x = max(x,0);
x = x / sum(x);

if display==1
display = Inf;
end
beta_ARMIJO = .25 ;
sigma_ARMIJO = .1 ;
kmax_ARMIJO = 20;
kmax = 200;
tol_dx = 1e-8;
k = 1;
alpha=1;

% % optimization parameters for the dual problem
% optparam.tol = 1e-16;
% optparam.kmax = 200;
% optparam.tol_df = 1e-20;
% optparam.display= 0;
fx0 = Inf;

% solve the regular kernel learning problem
mY = mean(Y);

%nu =  1e-6; % added ridge on kernels
while k<=kmax;
    x = max(x,0);
    x = x / sum(x);

    % compute value of function
    eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
    zeta = zeros(p,1);
    for i=1:p
        zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
    end
    zeta = 1./zeta;

    % solve the regular kernel learning problem
    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fx =  lambda/2 * ( Y - mean(Y))'* alphaK;

    gradient_zeta = zeros(p,1);
    for i=1:p
        gradient_zeta(i) = - lambda/2 *  vectorize_quad_single(data.kernels(:,i),alphaK );
    end

    gradient_eta = zeros(p,1);
    for i=1:p;
        gradient_eta(i) = sum( gradient_zeta(affs{i}) .* zeta(affs{i}).^2 ) / eta(i)^2;
    end
    gradient = (1-gapeta) * gradient_eta ./ weights.^2;

    % check optimality condition
    optcond = max(x .* ( gradient - min(gradient)));
    if max(x .* ( gradient - min(gradient))) < mingap, break; end

    fx0=fx;

    % projected gradient
    xbar = x -   gradient;
    d = - gradient;
    x0=x;
    ialpha = 1;
    while ialpha<kmax_ARMIJO
        xalpha = simplex_projection(x + d * alpha);
        % fxalpha = feval(fun,xalpha,varargin{:});
        eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
        zeta = zeros(p,1);
        for i=1:p
            zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
        end
        zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fxalpha =  lambda/2 * ( Y - mean(Y))'* alphaK;

        if fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x );

            % start to go up
            while ( fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x ) ) & ialpha<kmax_ARMIJO;
                alpha = alpha / beta_ARMIJO;
                xalpha = simplex_projection(x + d * alpha);
                %fxalpha = feval(fun,xalpha,varargin{:});
                eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
                zeta = zeros(p,1);
                for i=1:p
                    zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
                end
                zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fxalpha =  lambda/2 * ( Y - mean(Y))'* alphaK;

                % fprintf('+');
                ialpha = ialpha+1;
            end
            alpha = alpha * beta_ARMIJO;
            xalpha = simplex_projection(x + d * alpha);
            x = xalpha;
            break;
        else
            alpha = alpha * beta_ARMIJO;

        end
        ialpha = ialpha+1;
    end

    if ialpha==kmax_ARMIJO, alpha=1; end

    if ~isinf(display)
        if display==1,
            fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);
        end
        if display>1,
            if mod(k,display)==1
                fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);

            end
        end
    end
    if norm(x-x0)<tol_dx,  break; end

    if sum(x) > 1+1e-1, keyboard; end

    k=k+1;
end

% if k==kmax+1
%     keyboard;
% end

eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
zeta = zeros(p,1);
for i=1:p
    zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
end
zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alpha = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );

    pred = K * alpha;
    b=mean(Y-pred );

kappa = zeros(p,p);
for i=1:p
    kappa(i,affs{i}) = zeta(affs{i})./eta(i);
end

 if isinf(display)
            fprintf('k=%d - f=%f -  gap=%f\n',k,fx,optcond);
        end


function M = diag(f)
%DIAG   Multiplication operator.
%   M = DIAG(F) returns a multiplication operator M so that M*u is equivalent to
%   F.*u.
%
%   This method is mainly provided for backwards compatibility.

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

M = operatorBlock.mult(f);

end


function clean_tetgen_mesh(input_file,output_file)
  % CLEAN_TETGEN_MESH TetGen writes ALL triangles to the .mesh file, that is
  % all faces of the tetrahedra in the 3D mesh.
  % clean_tetgen_mesh(input_file,output_file) reads the vertices and tetrahedra
  % in input_file (ignoring triangles) and determines the surfaces triangles
  % then write the vertices, surfaces triangles and tetrahedra to output_file
  %
  % clean_tetgen_mesh(input_file,output_file)
  %
  % Input:
  %   input_file  path to .mesh file containing vertices and tetrahedra
  %   output_file  path to .mesh file to be written
  %
  [V,T,F] = readMESH(input_file);
  F = boundary_faces(T);
  % reverse orientation of triangles, tetgen seems to have backwards
  % orientation
  F = [F(:,3) F(:,2) F(:,1)];
  % rearrange vertices so that vertices on surface come before internal
  % vertices
  [V,T,F] = faces_first(V,T,F);
  writeMESH(output_file,V,T,F);
end


function Z = ne(X,Y)
%NE Not equal (~=) for tensors.
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

Z = tenfun(@ne,X,Y);


%  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 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function tfrrppat
%TFRRPPAT Unit test for the function TFRRPPAG.

%	O. Lemoine - April 1996.

N=128;

% Reality of the TFR
sig=noisecg(N);
[tfr,rtfr]=tfrrppag(sig);
if sum(any(abs(imag(rtfr))>sqrt(eps)))~=0,
 error('tfrrppag test 1 failed');
end

% Energy conservation
sig=fmlin(N);
[tfr,rtfr]=tfrrppag(sig);
Es=norm(sig)^2;
Etfr=sum(mean(rtfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrrppag test 2 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
[tfr,rtfr]=tfrrppag(sig);
[ik,jk]=find(abs(rtfr)>sqrt(eps));
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrrppag test 3 failed');
end;

% frequency localization
f0=30;
sig=fmconst(N+6,f0/N);
[tfr rtfr]=tfrrppag(sig,N/2+2,N,tftb_window(N+1,'rect'));
if any(find(rtfr>2*max(rtfr)/N)~=f0+1)|(abs(mean(rtfr)-1.0)>sqrt(eps)),
 error('tfrrppag test 4 failed');
end;

N=131;

% Reality of the TFR
sig=noisecg(N);
[tfr,rtfr]=tfrrppag(sig);
if sum(any(abs(imag(rtfr))>sqrt(eps)))~=0,
 error('tfrrppag test 5 failed');
end

% Energy conservation
sig=fmlin(N);
[tfr,rtfr]=tfrrppag(sig);
Es=norm(sig)^2;
Etfr=sum(mean(rtfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrrppag test 6 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
[tfr,rtfr]=tfrrppag(sig);
[ik,jk]=find(abs(rtfr)>sqrt(eps));
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrrppag test 7 failed');
end;

% frequency localization
f0=30;
sig=fmconst(N+6,f0/N);
[tfr rtfr]=tfrrppag(sig,round(N/2)+2,N,tftb_window(N,'rect'));
if any(find(rtfr>2*max(rtfr)/N)~=f0+1)|(abs(mean(rtfr)-1.0)>sqrt(eps)),
 error('tfrrppag test 8 failed');
end;


questdlg('Please selelct a polygon on the cross-section', ...
    '3D grid selection', ...
    'OK','Cancel');

figure_w_normalized_uicontrolunits(xsec_fig)

ax = findobj('Tag','main_map_ax');
[x,y, mouse_points_overlay] = select_polygon(ax);
zmap_message_center.set_info('Message',' Thank you .... ')

plos2 = plot(x,y,'b-','era','xor');        % plot outline
sum3 = 0.;
pause(0.3)

%create a rectangular grid
xvect=[min(x):dx:max(x)];
zvect=[min(y):dy:max(y)];
yvect=[z2:dz:z1];
gx = xvect;
gy= yvect;
gz= zvect;
tmpgri=zeros((length(xvect)*length(zvect)),2);
tmpgri2=zeros((length(xvect)*length(zvect)),2);

n=0;
for i=1:length(xvect)
    for j=1:length(zvect)
        n=n+1;
        tmpgri(n,:)=[xvect(i) zvect(j)];
        tmpgri2(n,:) = [i j ];
    end
end

%extract all gridpoints in chosen polygon
XI=tmpgri(:,1);
YI=tmpgri(:,2);

    ll = polygon_filter(x,y, XI, YI, 'inside');
%grid points in polygon
newgri=tmpgri(ll,:);

n2 = repmat(tmpgri,length(yvect),1);
k = repmat(yvect,length(tmpgri),1);
k = reshape(k,length(k)*length(yvect),1);
k2 = repmat(ll,length(yvect),1);
t3 = [n2 k k2];

n3 = repmat(tmpgri2,length(yvect),1);
k = repmat((1:1:length(yvect)),length(tmpgri),1);
k = reshape(k,length(k)*length(yvect),1);
t4 = [t3 n3 k];

l = t4(:,4) == 1;
t5 = t4(l,:);

% plot the grid points
figure_w_normalized_uicontrolunits(xsec_fig)
pl = plot(newgri(:,1),newgri(:,2),'+k','era','normal');
set(pl,'MarkerSize',8,'LineWidth',1)
drawnow


function varargout = xyz
%XYZ   Three chebfun3 obejcts of the identity on [-1, 1, -1, 1, -1, 1].
%   CHEB.XYZ is shorthand for the expressions
%   X = CHEBFUN3(@(X,Y,Z) X),
%   Y = CHEBFUN3(@(X,Y,Z) Y), and
%   Z = CHEBFUN3(@(X,Y,Z) Z).

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

x = chebfun3(@(x,y,z) x);
y = chebfun3(@(x,y,z) y);
z = chebfun3(@(x,y,z) z);

if nargout>1
    % For the syntax [x,y,z] = cheb.xyz
    varargout{1} = x;
    varargout{2} = y;
    varargout{3} = z;
else
    % For the syntax cheb.xyz we still want to put these variables into the
    % workspace:
    assignin('base', 'x', x);
    assignin('base', 'y', y);
    assignin('base', 'z', z);
end

end


function model = robOneDynamicsCreate(q, d, latentVals)

% ROBONEDYNAMICSCREATE Create the dynamics model.

% FGPLVM

if(q~=2)
  error('Robot One Dynamics designed for 2-D latent spaces.');
end
if(d~=q)
  error('Input dimension must equal output dimension');
end
model.a = 20;
model.sigma2 = (pi/16).^2;
model.mixTheta = 0.8;
model.mixR = 0.5;

model = robOneDynamicsSetLatentValues(model, latentVals);

% set b (the scale of the gamma distributions) so that a/b is the average jump.
aveR = mean(model.r);
model.b = model.a/aveR;
model.type = 'robOneDynamics';