classdef ParadigmRWSRCSP < ParadigmBase
    % Riemannian Weighted Subjects Regularized CSP (RWSRCSP)
    %
    % This paradigm implements RWSRCSP [1], which is a generalization of the Common Spatial Patterns
    % algorithms to calibration data comprising multiple subjects (or recordings). To train a model
    % for a particular "goal" (or target) subject using auxiliary data from other subjects, this
    % algorithm blends the subject-specific covariance matrices used in standard CSP and LDA with
    % weighted averages of covariance matrices from other subjects. The weighting is determined
    % based on the riemannian distance between the target subject and each respective other subject.
    % Also, the method can optionally average predictions obtained with different values of the
    % regularization parameter instead of doing a (costly) parameter search as suggested in [1].
    %
    % References:
    % [1] Lotte, F.
    %     "Signal processing approaches to minimize or suppress calibration time in oscillatory activity-based Brain-Computer Interfaces",
    %     Proceedings of the IEEE, vol. 103, no. 6, pp. 871-890, 2015
    %
    % Name:
    %   Riemannian Weighted Subjects Regularized Common Spatial Patterns
    %
    %                            Christian Kothe, Syntrogi
    %                            2015-07-24

    methods

        function defaults = preprocessing_defaults(self)
            defaults = {'FIRFilter',{'Frequencies',[6 8 28 32],'Type','minimum-phase'}, 'EpochExtraction',[0.5 3.5], 'Resampling',100};
        end

        function defaults = machine_learning_defaults(self)
            % set up the default parameters for machine learning; not necessary
            defaults = {'lda'};
        end

        function model = calibrate(self,varargin)
            % calibrate an SSRCSP model from a corpus of training sets
            args = arg_define(varargin, ...
                arg_norep({'collection','Collection'}), ...
                arg_norep({'goal_identifier','GoalIdentifier'}), ...
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 10000]),'Number of CSP patterns (times two).'),...
                arg({'lambdas','Lambdas'},0.1:0.1:0.9,[0 1],'Covariance shrinkage. A range of shrinkage parameters to run over (classifiers will be averaged).'),...
                arg({'group_tasks_by','GroupTasksBy'},'subject',{'group','subject','day','montage','session','recording','block'},'Group tasks into. This allows to group the training data into tasks solved in a multi-task manner, e.g., such that data of a given subject forms one task. When hyper-parameters need to be optimized, this would usually be done using a basic blockwise cross-validation within each task.'), ...
                arg({'shrinkage_cov','ShrinkageCovariance','ShrinkageCov'},false,[],'Shrinkage covariance estimator. Whether to use shrinkage to estimate the covariance matrices.'), ...
                arg({'weight_bias','WeightedBias'}, false, [], 'Account for class priors in bias. If you do have unequal probabilities for the different classes, this should be enabled.'), ...
                arg({'weight_cov','WeightedCov'}, false, [], 'Account for class priors in covariance. If you do have unequal probabilities for the different classes, it makes sense to enable this.'), ...
                arg({'apply_to','ApplyTo'},'channels',{'channels','sources','components','full CSD'},'Apply classifier to. Allows to select the type of time series to apply this model to.'), ...
                arg_sub({'flt','SignalProcessing'}, self.preprocessing_defaults(), @flt_pipeline, 'Signal processing stages. These parameters control filter stages that run on the signal level; they can be enabled, disabled and configured for the given paradigm. The prediction operates on the outputs of this stage.'), ...
                arg_sub({'ml','MachineLearning'},{'Learner',self.machine_learning_defaults()},@ml_train,'Machine learning stage of the paradigm. Operates on the feature vectors that are produced by the feature-extraction stage.'),...
                arg({'arg_dialogsel','ConfigLayout'},self.dialog_layout_defaults(),[],'Parameters displayed in the config dialog. Cell array of parameter names to display (dot-notation allowed); blanks are translated into empty rows in the dialog. Referring to a structure argument lists all parameters of that struture, except if it is a switchable structure - in this case, a pulldown menu with switch options is displayed.','type','cellstr','shape','row'));

            % if this is run on a worker, we'll set the cache capacity to zero since no machine
            % has enough RAM to hold multiple workers' copies of the corpus in memory
            on_worker = hlp_iscaller('par_worker');
            if on_worker
                global tracking;
                tracking.cache.capacity = 0;
            end

            % split data into reference data (of goal subject) and remaining data
            [refsets,remaining] = utl_collection_closest(args.collection,args.goal_identifier);

            % recombine and move ref data to the beginning (because this paradigm will later
            % extract the weights learned for the first task) and turn into struct array for
            % convenience
            corpus = [refsets{:} remaining{:}];

            % determine group membership
            group_membership = {corpus.(args.group_tasks_by)};
            if iscellstr(group_membership)
                groups = unique(group_membership);
            else
                groups = num2cell(unique([group_membership{:}]));
            end

            % initialize per-class covariance matrices
            CovCSP = deal(cell(2,length(groups)));
            CovLDA = deal(cell(2,length(groups)));
            % for each group...
            for s=length(groups):-1:1
                matches = find(cellfun(@(x)isequal(x,groups{s}),group_membership));
                matchdata = cell(1,length(matches));

                % collect all matching data sets...
                for p=1:length(matches)
                    matchdata{p} = corpus(matches(p));
                    if length(matchdata{p}.streams) > 1
                    disp_once('Note: ParadigmRWCSP will use only the first data stream of a recording (no support for multi-modal data).'); end
                    matchdata{p} = matchdata{p}.streams{1};
                end

                % concatenate them into a single set
				procdata = set_concat(matchdata{:});
				% and preprocess the result (with further settings/overrides according to args.flt)
                procdata = flt_pipeline('Signal',procdata, args.flt); %#ok<*NODEF>
                if on_worker
                    % if we're running on a worker we don't cache the result due to memory
                    % constraints
                    procdata = exp_eval(procdata);
                else
                    procdata = exp_eval_optimized(procdata);
                end
                % extract data
                switch args.apply_to
                    case 'channels'
                        X = procdata.data;
                    case 'components'
                        X = reshape((procdata.icaweights*procdata.icasphere)*procdata.data(procdata.icachansind,:),[],procdata.pnts,procdata.trials);
                    case 'sources'
                        X = procdata.srcpot;
                    case 'full CSD'
                        X = procdata.srcpot_all;
                end
                X(~isfinite(X(:))) = 0;
                for k=1:2
                    trials{k} = exp_eval(set_picktrials(procdata,'rank',k));
                    % calculate the CSP class covariance matrix
                    if args.shrinkage_cov
                        CovCSP{k,s} = hlp_diskcache('featuremodels',@cov_shrink,reshape(trials{k}.data,size(trials{k}.data,1),[])');
                    else
                        CovCSP{k,s} = cov(reshape(trials{k}.data,size(trials{k}.data,1),[])');
                    end
                    CovCSP{k,s}(~isfinite(CovCSP{k,s})) = 0;
                end
                % solve CSP
                [V,D] = eig(CovCSP{1,s},CovCSP{1,s}+CovCSP{2,s}); %#ok<NASGU>
                featuremodel.filters = V(:,[1:args.patterns end-args.patterns+1:end]);
                features = self.feature_extract(procdata,featuremodel); % nT x nF
                % get the LDA covariance matrix per class
                %targets = [signal.epoch.target];
                for k=1:2
                    CovLDA{k,s} = cov(features([trials{k}.epoch.targnum],:)); end
            end

%             % check for and remove bad data
%             remove = [];
%             for s=1:length(features)
%                 if size(features{s},3) < 2
%                     fprintf('Encountered bad data at subject %s/%i.\n',corpus(s).streams{1}.parts{2}.parts{1:2});
%                     remove(end+1) = s;
%                 end
%             end
%             if ~isempty(remove)
%                 fprintf('Removing bad data...\n');
%                 scales(remove) = [];
%                 features(remove) = [];
%                 targets(remove) = [];
%                 transforms(remove) = [];
%                 % data_weights(remove) = [];
%             end

            % calculate Riemannian weights for each subject
            Covs = vertcat(CovCSP,CovLDA);
            % for each type of covariance matrix...
            for c=size(Covs,1):-1:1
                Ctarg = Covs{c,1};

                % calc distance to all other covs
                for s=size(Covs,2):-1:2
                    Cother = Covs{c,s};
                    [V,D] = eig(Ctarg\Cother);
                    dist(s) = sqrt(sum(log(diag(D)).^2));
                end

                % calc weighting
                sumdist = sum(dist(2:end));
                for s=2:size(Covs,2)
                    weights(s) = 1./(dist(s)/sumdist); end

                % use it to avg the covs
                Avg = zeros(length(Ctarg));
                for s=2:size(Covs,2)
                    Avg = Avg + weights(s)*Covs{c,s}; end
                AvgCovs{c} = Avg;
            end

            % for each reg. param, solve a classifier
            signal = procdata;
            targets = [signal.epoch.target];
            classes = unique(targets);
            for li = length(args.lambdas):-1:1
                lam = args.lambdas(li);
                for c=size(Covs,1):-1:1
                    BlendCovs{c} = lam*Covs{c,1} + (1-lam)*AvgCovs{c}; end
                [V,D] = eig(BlendCovs{1},BlendCovs{1}+BlendCovs{2}); P = inv(V); %#ok<NASGU>
                % train CSP part
                model.featuremodel.filters{li} = V(:,[1:args.patterns end-args.patterns+1:end]);
                model.featuremodel.patterns{li} = P([1:args.patterns end-args.patterns+1:end],:);
                % extract features
                trials = self.feature_extract(signal,model.featuremodel,li);
                % train LDA part
                for c = 1:2
                    X = trials(targets==classes(c),:);
                    n{c} = size(X,1);
                    mu{c} = mean(X,1);
                    sig{c} = BlendCovs{c+2};
                end
                ns = quickif(args.weight_cov,n,{1 1});
                nb = quickif(args.weight_bias,n,{1 1});
                % do the math
                mu_both = (mu{1}*nb{2} + mu{2}*nb{1}) / (nb{1}+nb{2});
                sig_both = (sig{1}*ns{1} + sig{2}*ns{2}) / (ns{1}+ns{2});
                w = (mu{2} - mu{1}) / sig_both;
                w = w / (mu{2}*w' - mu_both*w');
                model.predictivemodel.model{li} = struct('w',{w}, 'b',{mu_both*w'}, 'classes',{classes});
            end

            % set the filter graph based on the reference data
            model.tracking.filter_graph = signal;
            % also store channel locations for model visualization
            model.chanlocs = signal.chanlocs;
            model.classes = classes;
        end

        function predictions = predict(self,bundle,model)
            % for each lambda
            for m=length(model.featuremodel.filters):-1:1
                % extract features
                features = self.feature_extract(bundle.streams{1},model.featuremodel.filters{m});
                % apply classifier
                raw_preds(:,m) = features*model.predictivemodel.model{m}.w' - model.predictivemodel.model{m}.b;
            end
            % average all predictions
            raw_labels = mean(raw_preds,2);
            raw_labels = min(+1,max(-1,raw_labels));
            predictions = {'disc', [(1-raw_labels)/2 1-(1-raw_labels)/2], model.classes};
        end

        function features = feature_extract(self,signal,featuremodel,lam)
            if isstruct(featuremodel)
                featuremodel = featuremodel.filters; end
            if iscell(featuremodel)
                featuremodel = featuremodel{lam}; end
            % extract log-variance features from an epoched and preprocessed recording
            features = zeros(size(signal.data,3),size(featuremodel,2));
            for t=1:size(signal.data,3)
                features(t,:) = sum((signal.data(:,:,t)'*featuremodel).^2,1); end
            features = log(features/size(signal.data,2));
        end

        function visualize(self,varargin) %#ok<*INUSD>
            % visualize an mklCSP model
            args = arg_define(varargin, ...
                arg_norep({'model','Model'},[],[],'BCI Model to visualize.'), ...
                arg({'patterns','PlotPatterns'},true,[],'Plot patterns instead of filters. Whether to plot spatial patterns (forward projections) rather than spatial filters.'), ...
                arg({'paper','PaperFigure'},false,[],'Use paper-style font sizes. Whether to generate a plot with font sizes etc. adjusted for paper.'));

            f = figure;
            % get number of pairs, and index of pattern per subplot
            np = size(args.model.featuremodel.patterns,1)/2;
            idx = [1:np 2*np:-1:np+1];
            % for each CSP pattern...
            for p=1:np*2
                subplot(2,np,p,'Parent',f);
                if args.patterns
                    topoplot(args.model.featuremodel.patterns(idx(p),:),args.model.featuremodel.chanlocs);
                else
                    topoplot(args.model.featuremodel.filters(:,idx(p)),args.model.featuremodel.chanlocs);
                end
                t = title(['CSP Pattern ' num2str(idx(p))]);
                if args.paper
                    set(t,'FontUnits','normalized');
                    set(t,'FontSize',0.1);
                end
            end
        end

        function layout = dialog_layout_defaults(self)
            % define the default configuration dialog layout
            layout = {'SignalProcessing.Resampling.SamplingRate', 'SignalProcessing.FIRFilter.Frequencies', ...
                'SignalProcessing.FIRFilter.Type', 'SignalProcessing.EpochExtraction', '', ...
                'PatternPairs', 'CovarianceShrinkage', '', 'MachineLearning.Learner'};
        end

    end
end

% (turn off a few editor warnings because some actual implementations are missing in this file)
%#ok<*INUSD,*STOUT,*MANU>


%==========================================================================
% [Saliency_Map, Feature_Maps, ICA_Maps, img, SUN] = Run_SUN(img, SUN)
% returns the SUN Saliency Map of img as well as other useful quantities.
%
% Input:    img is a color (preferably) or black and white image.
%           SUN (optional) is a structure containing SUN's parameters.
%
% Output:   Saliency_Map is a saliency map created out of ICA responses
%               using the SUN framework.
%           Feature_Maps contains ICA filter responses nonlinearly
%               weighted by their statistical frequency.
%           ICA_Maps contains the ICA filter responses with the sign
%               discarded.
%           img is the image after being preprocessed.
%           SUN is a structure that contains SUN's parameters.
%
% Note:     If you are making subsequent calls to this function, pass
%           an empty array to SUN and then use the returned version. This
%           will substantially speed up the function.
%
% Example:  img = imread('wpeppers.jpg');figure;
%           subplot(1, 4, 1);imagesc(img);title('Original Image');axis off;
%           [Saliency_Map, Feature_Maps, ICA_Maps, img] = ...
%               Run_SUN(img, []);
%           subplot(1, 4, 2);imagesc(img);title('Preprocessed Image');
%           axis off;
%           subplot(1, 4, 3);imagesc(Saliency_Map);title('Saliency Map');
%           axis off;subplot(1, 4, 4);imagesc(mean(Feature_Maps, 3));axis off;
%           title('Mean Feature Map');
%
%
% Author:   Christopher Kanan (chriskanan@gmail.com)
%
% Cite as:  Kanan, C. & Cottrell, G. W. (2010). Robust classification of
%             objects, faces, and flowers using natural image Statistics.
%             In Proceedings of the IEEE Conference on Computer Vision
%             and Pattern Recognition (CVPR), 2010.
%==========================================================================
function [Saliency_Map, Feature_Maps, ICA_Maps, img, SUN] = Run_SUN(img, SUN, max_filters, img_size, skip_preprocessing)

%set up default parameters
if ~exist('img_size', 'var')
    img_size = 128; %default canonical image size
end

pad_image = false;
use_probit = false; %keep false in general for more discriminability
approx_gammainc = true; %makes the algorithm much faster

if nargin == 0
    img = imread('wpeppers.jpg');
    orig = img;
    img_size = 192;
end

if ~exist('img', 'var')
    error('No image was passed to SUN_RICA.');
end

if ~exist('SUN', 'var') || isempty(SUN)
    SUN = load('SUN.mat');
    if isfield(SUN, 'SUN')
        SUN = SUN.SUN;
    end
end

if ~exist('max_filters', 'var')
    max_filters = inf;
end
Num_Filters = min(SUN.Num_Filters, max_filters);

if approx_gammainc && isempty(SUN.Filters(1).Inc_Gamma_List_X)
    %this takes a few seconds, but only needs to be repeated once as long
    %as you use the returned SUN structure on subsequent function calls
    for f = 1:Num_Filters
        x_max = SUN.Filters(f).x_max;
        x_inc = SUN.Filters(f).x_inc;
        SUN.Filters(f).Inc_Gamma_List_X = single(0:x_inc:x_max)';
        SUN.Filters(f).Inc_Gamma_List_Y = single(gammainc(SUN.Filters(f).Inc_Gamma_List_X, 1 ./ SUN.Theta(f), 'lower'));
    end
end

%preprocess the image to make it into the "canonical" size
if ~exist('skip_preprocessing', 'var') || ~skip_preprocessing
    img = Preprocess_Image(img, img_size);
end

Filters = SUN.Filters;

Filt_Size = size(Filters(1).R, 1);
half_filt = Filt_Size / 2;
if pad_image
    img = padarray(img, [half_filt, half_filt], 'both', 'replicate');
end

[SY, SX, SZ] = size(img);
prec = 'single';

%subtract chanel means
R = img(:, :, 1) - SUN.Channel_Mean(1);
G = img(:, :, 2) - SUN.Channel_Mean(2);
B = img(:, :, 3) - SUN.Channel_Mean(3);

%prepare image for FFT
FFT_R = fftn(R);
FFT_G = fftn(G);
FFT_B = fftn(B);

%prepare filters for FFT
for f = 1:Num_Filters
    if isempty(Filters(f).FFT_R) || size(Filters(f).FFT_R, 1) ~= SY || size(Filters(f).FFT_R, 2) ~= SX
        Filt_Size = size(Filters(f).R, 1);
        Filters(f).FFT_R = zeros(size(img, 1), size(img, 2), prec);
        Filters(f).FFT_G = zeros(size(img, 1), size(img, 2), prec);
        Filters(f).FFT_B = zeros(size(img, 1), size(img, 2), prec);

        Filters(f).FFT_R(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).R));
        Filters(f).FFT_G(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).G));
        Filters(f).FFT_B(1:Filt_Size, 1:Filt_Size) = fliplr(flipud(Filters(f).B));

        Filters(f).FFT_R = fftn(Filters(f).FFT_R);
        Filters(f).FFT_G = fftn(Filters(f).FFT_G);
        Filters(f).FFT_B = fftn(Filters(f).FFT_B);
    end
end

Filt_Size = size(Filters(1).R, 1);
shape_flag = 'valid';

if strcmpi(shape_flag, 'valid')
    deduct = Filt_Size;
else
    deduct = 0;
end

%pre-allocate memory for speed
ICA_Maps = zeros(SY - deduct, SX - deduct, Num_Filters, prec);
Feature_Maps = zeros(SY - deduct, SX  - deduct, Num_Filters, prec);
SUN_Maps = zeros(SY  - deduct, SX  - deduct, Num_Filters, prec);

[SY, SX, SZ] = size(Feature_Maps);

%get the filter responses
inv_theta = 1 ./ SUN.Theta(f);
for f = 1:Num_Filters
    theta = SUN.Theta(f);
    sigma = SUN.Sigma(f);

    %do FFT filtering
    filt_resp = (ifftn(Filters(f).FFT_R .* FFT_R + Filters(f).FFT_G .* FFT_G + Filters(f).FFT_B .* FFT_B, 'symmetric'));
    filt_resp = filt_resp(Filt_Size+1:end, Filt_Size+1:end);
    filt_resp = abs(single(filt_resp));
    if deduct == 0
        filt_resp = imresize(filt_resp, [SY, SX], 'nearest');
    end

    filt_resp = filt_resp / max(filt_resp(:));
    ICA_Maps(:, :, f) = filt_resp;
    filt_resp = (filt_resp ./  sigma) .^ theta;
    SUN_Maps(:, :, f) = filt_resp;

    if ~approx_gammainc
        R_Map = gammainc(filt_resp, inv_theta(f), 'lower');
    else
        Yi = qinterp1c(Filters(f).Inc_Gamma_List_X, Filters(f).Inc_Gamma_List_Y, filt_resp);
        R_Map = reshape(Yi, [SY, SX]);
    end

    %R_Map = R_Map ./ max(abs(R_Map(:)));
    Feature_Maps(:, :, f) = R_Map;
end

if use_probit
    Feature_Maps = norminv(Feature_Maps);
end

Feature_Maps(~isfinite(Feature_Maps)) = 0;

Saliency_Map = sum(SUN_Maps, 3);

Saliency_Map = double(Saliency_Map);

%this actually alters the saliency map, but I do it here for numerical purposes
Saliency_Map = Saliency_Map - min(Saliency_Map(:));
Saliency_Map = Saliency_Map ./ max(Saliency_Map(:));

%some filtering to smooth the saliency map
sz = 9;
h = fspecial('gaussian', sz, sz / 2);
Saliency_Map = imfilter(Saliency_Map, h, 'conv');
Saliency_Map = exp(Saliency_Map - log(sum(exp(Saliency_Map(:)))));

SUN.Filters = Filters;

if nargin == 0
    figure;
    subplot(1, 2, 1)
    imagesc(orig);
    title('Input Image');
    axis off
    subplot(1, 2, 2);
    imagesc(Saliency_Map);
    axis off;
    title('Saliency Map');
    img = [];
    Saliency_Map = [];
    Feature_Maps = [];
    ICA_Maps = [];
    SUN = [];
end

return;

%==========================================================================
% This function converts to LMS space and applies the log nonlinearity to
% to the image.
%
% Author:   Christopher Kanan (chriskanan@gmail.com)
%==========================================================================
function img = Preprocess_Image(img, img_size)

do_smoothing = true;

img = single(img);
if max(img(:) > 1)
    img = img ./ 255;
end

if do_smoothing
    %this helps with our noisy JPEG images, and is similar to some
    %computations done in the retina
    sz_per = 0.03;
    sz = max(floor(sz_per*min(size(img, 1), size(img, 2))), 3);
    sz = sz + (mod(sz, 2) == 0);h = fspecial('gaussian', sz, 1);
    img = imfilter(img, h, 'same');
end

if ndims(img) == 2 %handle grayscale images by making them color
    img = repmat(img, [1, 1, 3]);
end

img = SRGB2LMS(img); %convert from sRGB to LMS color space

[sy, sx, sz] = size(img);
rescale_factor = img_size / min(sy, sx);
img = imresize(img, round([sy, sx]*rescale_factor));

img = min(max(img, 0), 1); %bicubic interpolation can make some values greater than 1 or less than 0

%normalize image
img = img - min(img(:));
img = img ./ max(img(:));
%img = min(max(img, 0), 1); %should do nothing

%now log nonlinearity
C = 0.005; %arbitrary choice
%img_log = (log((img + C) ./ C)) ./ (log((1 + C) ./ C)); %output is in [0,1]
img_log = (log(img + C) - log(C)) ./ (log(1 + C) - log(C));
img_log = max(img_log, 0); %in case of numerical issues

img = img_log;

img(isnan(img)) = 0; %This should never be needed

%==================================================================
% This function converts from the default standard RGB color space
% to LMS color space, which approximates the response of
% human cone photoreceptors
%
% Input: A color image in SRGB color space, the default using
% imread(.), either as an NxMx3 matrix or as a (N*M)x3 matrix where
% each column represent a color channel
%
% Output: The image in LMS color space
%
% Example:  This will do nothing, since the functions are inverses,
%           peppers = imread('wpeppers.jpg');
%           figure;
%           subplot(1, 3, 1);imshow(peppers);
%           title('Original (SRGB)');
%           subplot(1, 3, 2);imshow(SRGB2LMS(peppers));
%           title('LMS Space');
%           subplot(1, 3, 3);imshow(LMS2SRGB(SRGB2LMS(peppers)));
%           title('Invert back to SRGB space')
%
% Author: Christopher Kanan (chriskanan@gmail.com)
%==================================================================
function LMS_Feat = SRGB2LMS(RGB_Feat)

XYZ_RGB = [0.4124, .3576, .1805; 0.2126, 0.7152, 0.0722; 0.0193, 0.1192, 0.9505];
XYZ_RGB = XYZ_RGB ./ repmat(sum(XYZ_RGB, 2), [1, 3]);
LMS_XYZ = [0.7328, 0.4296, -0.1624; -0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834];
LMS_RGB = LMS_XYZ * XYZ_RGB; %CIECAM02

%see if we need to reshape the values
if size(RGB_Feat, 3) == 3 || size(RGB_Feat, 2) ~= 3
    is_image = true;
    [sy, sx, sz] = size(RGB_Feat);
    R = RGB_Feat(:, :, 1);
    G = RGB_Feat(:, :, 2);
    B = RGB_Feat(:, :, 3);
    RGB_Feat = [R(:), G(:), B(:)];
else
    is_image = false;
end
RGB_Feat = single(RGB_Feat);

% normalize:
if any(RGB_Feat(:) > 1)
    RGB_Feat = RGB_Feat ./ 255;
end

% remove sRGB gamma nonlinearity:
mask = RGB_Feat <= 0.04045;
RGB_Feat(mask) = RGB_Feat(mask) ./ 12.92;
RGB_Feat(~mask) = ((RGB_Feat(~mask) + 0.055) ./ 1.055) .^ 2.4;

%now do the conversion
LMS_Feat = (LMS_RGB * RGB_Feat')';

LMS_Feat = max(LMS_Feat, 0);
LMS_Feat = min(LMS_Feat, 1);

%turn back into an image if necessary
if is_image
    L = reshape(LMS_Feat(:, 1), [sy, sx]);
    M = reshape(LMS_Feat(:, 2), [sy, sx]);
    S = reshape(LMS_Feat(:, 3), [sy, sx]);
    LMS_Feat = zeros(sy, sx, 3, 'single');
    LMS_Feat(:, :, 1) = L;
    LMS_Feat(:, :, 2) = M;
    LMS_Feat(:, :, 3) = S;
end

%This is used for nearest neighbor interpolation
function Yi = qinterp1c(x,Y,xi)

% Forces vectors to be columns
x = x(:); xi = xi(:);
sx = size(x); sY = size(Y);
if sx(1)~=sY(1)
    if sx(1)==sY(2)
        Y = Y';
    else
        error('x and Y must have the same number of rows');
    end
end

% Gets the x spacing
warning('off');
ndx = 1/(x(2)-x(1)); % one over to perform divide only once
warning('on');
if x(1) ~= 0
    xi = xi - x(1);      % subtract minimum of x
end

rxi = round(xi*ndx)+1;        % indices of nearest-neighbors
len = length(x);
rxi(rxi < 1) = 1;
rxi(rxi > len) = len;
Yi = Y(rxi, :);


function [Nimages, NimagesFullyAnnotated, NumberOfAnnotatedObjects, NumberOfAnnotatedObjectsWithParts, NumberOfParts] = SUNstats(D)
%
%
% Number of images
% Number of fully annotated images
% Number of annotated objects
% Number of annotated root objects with parts
% Number of parts

% Number of images
Nimages = length(D);

% Number of fully annotated images
disp('Counting fully labeled images')
relativearea = LMlabeledarea(D);
NimagesFullyAnnotated = sum(relativearea>.9);

% Number of annotated objects

% Number of annotated root objects with parts
[counts, countrootwithparts, countparts] = LMcountparts(D);

NumberOfAnnotatedObjects = sum(counts);
NumberOfAnnotatedObjectsWithParts = sum(countrootwithparts);
NumberOfParts = sum(countparts);

fprintf('Number of images= %d \n', Nimages)
fprintf('Number of images (90%%) labeled = %d \n', NimagesFullyAnnotated)
fprintf('Number of annotated objects = %d \n', NumberOfAnnotatedObjects)
fprintf('Number of annotated objects with parts = %d \n', NumberOfAnnotatedObjectsWithParts)
fprintf('Number of annotated parts = %d \n', NumberOfParts)


function g = noiseGradientParam(noise, mu, varsigma, y)

% NOISEGRADIENTPARAM Gradient wrt the noise model's parameters.
% FORMAT
% DESC computes the gradient of the log Z of the given noise model
% with respect to the of functions with respect to the given
% noise model's parameters.
% ARG noise : the noise structure for which the gradients are being
% computed.
% ARG mu : the input means for which the gradients are being computed.
% ARG varSigma : the input variances for which the gradients are being computed.
% ARG y : the target values for the noise model.
% RETURN g : gradients of the log probability with respect to
% the noise parameters. The ordering of the vector should match
% that provided by the function noiseExtractParam.
%
%
% SEEALSO noiseCreate, noiseParamInit, noiseGradVals, noiseGradientParam
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

fhandle = str2func([noise.type 'NoiseGradientParam']);
g = fhandle(noise, mu, varsigma, y);

% check if there is a prior over parameters
if isfield(noise, 'priors')
  for i = 1:length(noise.priors)
    index = noise.priors(i).index;
    g(index) = g(index) + priorGradient(noise.priors(i), params(index));
  end
end

% Check if parameters are being optimised in a transformed space.
if isfield(noise, 'transforms')
  fhandle = str2func([noise.type 'NoiseExtractParam']);
  params = fhandle(noise);
  for i = 1:length(noise.transforms)
    index = noise.transforms(i).index;
    fhandle = str2func([noise.transforms(i).type 'Transform']);
    g(index) = g(index).*fhandle(params(index), 'gradfact');
  end
end


function tests = test_ft_connectivity_corr

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_connectivity_corr

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  func = localfunctions;
  for i=1:numel(func)
    fprintf('evaluating %s\n', func2str(func{i}));
    feval(func{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function test_rpt_chan_chan(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nrpt   = 10;
nchan  = 3;
ntime  = 100;
dimord = 'rpt_chan_chan';

input = zeros(nrpt, nchan, nchan);
for i=1:nrpt
  dat = 10 * randn(nchan, ntime);
  input(i,:,:) = cov(dat');
end

result = {};
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', false);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1:2);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1:3);

assert(all(diag(result{2})==1));
assert(all(size(result{2})==[3 3]));
assert(all(size(result{3})==[2 2]));
assert(all(size(result{4})==[1 1]));
assert(all(size(result{5})==[0 0]));

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequaln(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function test_rpt_chan_chan_time(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nrpt   = 10;
nchan  = 3;
nseg   = 20;  % sliding window, there are 20 segments per trial
ntime  = 50; % sliding window, there are 50 samples per segment
dimord = 'rpt_chan_chan_time';

input = zeros(nrpt, nchan, nchan);
for rpt=1:nrpt
  for seg=1:nseg
    dat = 10 * randn(nchan, ntime);
    input(rpt,:,:,seg) = cov(dat');
  end
end

result = {};
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', false);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1, 'allchanindx', 1:3);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 2, 'allchanindx', 1:3);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1:2, 'allchanindx', 1:3);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequaln(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function test_rpt_chan_chan_freq(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nrpt   = 10;
nchan  = 3;
nfreq  = 5;
dimord = 'rpt_chan_chan_freq';

input = zeros(nrpt, nchan, nchan, nfreq);
for rpt=1:nrpt
  for freq=1:nfreq
    fdat = 10 * randn(nchan, 1) + 1i * 10 * randn(nchan, 1);
    input(rpt,:,:,freq) = fdat * ctranspose(fdat); % compute the cross-spectral density
  end
end

result = {};
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', false);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1, 'allchanindx', 1:3);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 2, 'allchanindx', 1:3);
result{end+1} = ft_connectivity_corr(input, 'dimord', dimord, 'hasjack', false, 'pownorm', true, 'pchanindx', 1:2, 'allchanindx', 1:3);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequaln(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


function [infixOut, notaVAR] = pref2inf(prefixIn)
%PREF2INF   Convert an expression on prefix form to infix form
%   [INFIXOUT, NOTAVAR] = PREF2INF(PREFIXIN) goes recursively through the
%   expression PREFIXIN, which is on prefix form. The output, INFIXOUT, is a
%   string, representing the expression on infix form. NOTAVAR is a string, that
%   corresponds to the second variable in the kernel for FRED and VOLT
%   operators.

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

prefCounter = 1;
NOTAVAROUT = [];
infixOut = getInfix();
notaVAR = NOTAVAROUT;
NOTAVAROUT = [];

function infixOut = getInfix()
%GETINFIX   Return the infix form.

next = char(prefixIn(prefCounter,2));

% Steep through the prefix expression, dealing with each different type of
% tokens separately.
if ( ~isempty(strmatch('OP', next)) )
    % Increase the counter.
    prefCounter = prefCounter + 1;

    % We are dealing with a binary operator, so obtain two infix expressions.
    exp1 = getInfix();
    exp2 = getInfix();

    % We now return different outputs depending on which operator we have.
    switch next
        case 'OP='
            infixOut = [exp1, '=', exp2];

        case 'OP+'
            if ( strcmp(exp1, '0') && strcmp(exp2, '0') )
                % Don't want to introduce unnecessary zeros.
                infixOut = '';
            elseif  ( strcmp(exp1, '0') )
                infixOut = exp2;
            elseif ( strcmp(exp2, '0') )
                infixOut = exp1;
            else
                infixOut = ['(', exp1, '+', exp2, ')'];
            end

        case 'OP-'
            if ( strcmp(exp1, '0') && strcmp(exp2, '0') )
                infixOut = '';
            elseif  ( strcmp(exp1, '0') )
                infixOut = ['-',exp2];
            elseif ( strcmp(exp2, '0') )
                infixOut = exp1;
            else
                infixOut = ['(', exp1, '-', exp2, ')'];
            end

        case 'OP*'
            % Check whether we have some special arguments, which we know we can
            % simplify.
            if ( strcmp(exp1, '1') && strcmp(exp2, '1') )
                infixOut = '1';
            elseif  ( strcmp(exp1, '1') )
                infixOut = exp2;
            elseif ( strcmp(exp2, '1') )
                infixOut = exp1;
            elseif  ( strcmp(exp1, '-1') )
                infixOut = ['-', exp2];
            elseif ( strcmp(exp2, '-1') )
                infixOut = ['-', exp1];
            elseif ( strcmp(exp1, '0') || strcmp(exp2, '0') )
                infixOut = '0';
            elseif ( strcmp(exp1, '-0') || strcmp(exp2, '-0') )
                infixOut = '0';
            else
                infixOut = ['(', exp1, '.*', exp2, ')'];
            end

        case 'OP/'
            if ( strcmp(exp2, '1') )
                % Division by 1 is trivial.
                infixOut = exp1;
            else
                infixOut = ['(', exp1, './', exp2, ')'];
            end

        case 'OP^'
            infixOut = [exp1, '.^(', exp2, ')'];

        case {'OP>', 'OP>=', 'OP<', 'OP<='}
            nextSym = next(3:end);
            infixOut = ['(', exp1, nextSym, exp2, ')'];
    end

elseif ( strcmp(next, 'FUNC1') )
    % A function which has one argument.
    nextFun = char(prefixIn(prefCounter, 1));
    prefCounter = prefCounter + 1;
    funcArg = getInfix();
    infixOut = [nextFun, '(', funcArg , ')'];

elseif ( strcmp(next, 'FUNC2') )
    % A function with two arguments.
    nextFun = char(prefixIn(prefCounter, 1));
    prefCounter = prefCounter + 1;
    % Obtain two infix expressions.
    funcArg1 = getInfix();
    funcArg2 = getInfix();

    % Some methods need a special treatment.
    if ( (strcmp(nextFun, 'diff') || strcmp(nextFun, 'cumsum')) && ...
            strcmp(funcArg2, '1') )
        infixOut = [nextFun, '(', funcArg1, ')'];

    elseif ( any(strcmp(nextFun, {'fred', 'volt'})) )
        % For fred() and volt(), we need to find what arguments denote the
        % kernel variable.
        [ignored1, yFredVar, ignored2, ignored3, xFredVar] = ...
            stringParser.lexer(funcArg1, 'bvp');
        anonStr = ['@(' xFredVar{1} ',' yFredVar{1} ')'];
        infixOut = [nextFun, '(', anonStr, funcArg1 , ',', funcArg2 ,  ')'];
        NOTAVAROUT = [NOTAVAROUT ;  yFredVar];

    else
        infixOut = [nextFun, '(', funcArg1 , ',', funcArg2 ,  ')'];
    end

elseif ( strcmp(next, 'FUNC3') )
    % Methods with three arguments.
    nextFun = char(prefixIn(prefCounter,1));
    prefCounter = prefCounter + 1;
    funcArg1 = getInfix();
    funcArg2 = getInfix();
    funcArg3 = getInfix();
    infixOut = [nextFun, '(', funcArg1 , ',', funcArg2, ',' , funcArg3 ')'];

elseif ( strcmp(next(1:end-1), 'DER') )
    % Differentiation via the ' symbol.
    prefCounter = prefCounter + 1;
    derivArg = getInfix();
    derivOrder = next(4:end);
    if ( ~strcmp(derivOrder, '1') )
        infixOut = ['diff(', derivArg, ',' derivOrder, ')'];
    else
        infixOut = ['diff(', derivArg, ')'];
    end

elseif ( ~isempty(strmatch('UN', next)) )
    % Unary operators
    nextUnary = char(prefixIn(prefCounter, 1));
    prefCounter = prefCounter + 1;
    unaryArg = getInfix();
    % Only care about unary -
    if ( strcmp(nextUnary, '-') )
        infixOut = [nextUnary, unaryArg];
    else
        infixOut = unaryArg;
    end

elseif ( strmatch(next, 'COMMA') )
    % The , symbol.
    prefCounter = prefCounter + 1;
    exp1 = getInfix();
    exp2 = getInfix();
    infixOut = ['(', exp1, ';', exp2, ')'];
else
    % We have arrived at a terminal token. Return it and increase the counter.
    infixOut = char(prefixIn(prefCounter,1));
    prefCounter = prefCounter + 1;
end

end

% Initialization for simplification.
newlen = length(infixOut);
len = inf;

% Do some simplifications on the returned string. In particular, we want to
% change -- to +, a -+ to -, a +- to -, and ++ to +.
while ( newlen ~= len )     % Loop until length doesn't change anymore.
    len = newlen;

    k = 1;
    while ( k < (numel(infixOut) - 1) )
        if ( strcmp(infixOut(k), '-') )
            if ( strcmp(infixOut(k+1), '-') )
                infixOut(k) = '+';
                infixOut(k+1) = [];
            elseif ( strcmp(infixOut(k+1), '+') )
                infixOut(k+1) = [];
            else
                k = k+1;
            end
        elseif ( strcmp(infixOut(k), '+') )
            if ( strcmp(infixOut(k+1), '-') )
                infixOut(k) = '-';
                infixOut(k+1) = [];
            elseif ( strcmp(infixOut(k+1), '+') )
                infixOut(k+1) = [];
            else
                k = k+1;
            end
        else
            k = k+1;
        end
    end

    newlen = length(infixOut);

end

% Clear global variables
prefixIn = [];
prefCounter = [];

end


function k = rbfardKernDiagCompute(kern, x)

% RBFARDKERNDIAGCOMPUTE Compute diagonal of RBFARD kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the automatic relevance determination radial basis function kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% SEEALSO : rbfardKernParamInit, kernDiagCompute, kernCreate, rbfardKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

k = repmat(kern.variance, size(x, 1), 1);


function b = dspsl ( ap, n, kpvt, b )

%*****************************************************************************80
%
%% DSPSL solves the real symmetric system factored by DSPFA.
%
%  Discussion:
%
%    To compute inverse(A) * C where C is a matrix with P columns:
%
%      call dspfa ( ap, n, kpvt, info )
%
%      if ( info /= 0 ) go to ...
%
%      do j = 1, p
%        call dspsl ( ap, n, kpvt, c(1,j) )
%      end do
%
%    A division by zero may occur if DSPCO has set RCOND == 0.0D+00
%    or DSPFA has set INFO /= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ( kind = 8 ) AP(N*(N+1)/2), the output from DSPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer KPVT(N), the pivot vector from DSPFA.
%
%    Input, real B(N), the right hand side.
%
%    Output, real B(N), the solution.
%

%
%  Loop backward applying the transformations and D inverse to B.
%
  k = n;
  ik = floor ( ( n * ( n - 1 ) ) / 2 );

  while ( 0 < k )

    kk = ik + k;

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )

        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-1) = daxpy ( k-1, b(k), ap(ik+1:ik+k-1), 1, b(1:k-1), 1 );

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / ap(kk);
      k = k - 1;
      ik = ik - k;

    else
%
%  2 x 2 pivot block.
%
      ikm1 = ik - ( k - 1 );

      if ( k ~= 2 )

        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k-1 )
          temp = b(k-1);
          b(k-1) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-2) = daxpy ( k-2, b(k), ap(ik+1:ik+k-2), 1, b(1:k-2), 1 );
        b(1:k-2) = daxpy ( k-2, b(k-1), ap(ikm1+1:ikm1+k-2), 1, b(1:k-2), 1 );

      end
%
%  Apply D inverse.
%
      km1k = ik + k - 1;
      kk = ik + k;
      ak = ap(kk) / ap(km1k);
      km1km1 = ikm1 + k - 1;
      akm1 = ap(km1km1) / ap(km1k);
      bk = b(k) / ap(km1k);
      bkm1 = b(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;
      ik = ik - ( k + 1 ) - k;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;
  ik = 0;

  while ( k <= n )

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + ddot ( k-1, ap(ik+1:ik+k-1), 1, b(1:k-1), 1 );
        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      ik = ik + k;
      k = k + 1;

    else
%
%  2 x 2 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + ddot ( k-1, ap(ik+1:ik+k-1), 1, b(1:k-1), 1 );
        ikp1 = ik + k;
        b(k+1) = b(k+1) + ddot ( k-1, ap(ikp1+1:ikp1+k-1), 1, b(1:k-1), 1 );
        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      ik = ik + k + k + 1;
      k = k + 2;

    end

  end

  return
end


%% Copyright (C) 2022 Alex Vong
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software 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 software; see the file COPYING.
%% If not, see <https://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym piecewise (@var{cond1}, @var{val1}, @var{cond2}, @var{val2}, @dots{})
%% @defmethodx @@sym piecewise (@var{cond1}, @var{val1}, @var{cond2}, @var{val2}, @dots{}, @var{else_val})
%% Construct piecewise function.
%%
%% The returned piecewise function evaluates to @var{val1} if @var{cond1}
%% holds, @var{val2} if @var{cond2} holds, @dots{} etc.  In the case where none
%% of the conditions hold, it evaluates to @var{else_val} if provided.  If
%% @var{else_val} is not provided, it evaluates to @code{nan}.
%%
%% Examples:
%% @example
%% @group
%% syms x real
%% f = piecewise (abs (x) < 1, exp (- 1 / (1 - x^2)), abs (x) >= 1, 0)
%%   @result{} f = (sym)
%%       ⎧  -1
%%       ⎪ ──────
%%       ⎪      2
%%       ⎨ 1 - x
%%       ⎪ℯ        for │x│ < 1
%%       ⎪
%%       ⎩   0      otherwise
%% @end group
%% @end example
%%
%% For this piecewise function, we can omit the redundant condition at the end:
%% @example
%% @group
%% syms x real
%% f = piecewise (abs (x) < 1, exp (- 1 / (1 - x^2)), 0)
%%   @result{} f = (sym)
%%       ⎧  -1
%%       ⎪ ──────
%%       ⎪      2
%%       ⎨ 1 - x
%%       ⎪ℯ        for │x│ < 1
%%       ⎪
%%       ⎩   0      otherwise
%% @end group
%% @end example
%%
%% @seealso{if}
%% @end defmethod

function f = piecewise (varargin)
  if nargin < 1
    print_usage ();
  end

  cmd = {'def pack_args_without_else(args):'
         '    return [(val, cond) for cond, val in unflatten(args, 2)]'
         'def pack_args(args):'
         '    if len(args) % 2 == 0:'
         '        return pack_args_without_else(args)'
         '    else:'
         '        return pack_args_without_else(args[:-1]) + [(args[-1], True)]'
         'return Piecewise(*pack_args(_ins))'
        };

  args = cellfun (@sym, varargin, 'UniformOutput', false);
  f = pycall_sympy__ (cmd, args{:});
end

%!test
%! % basic
%! syms x real
%! f = piecewise (abs (x) < 1, 1);
%! assert (isnan (subs (f, -1)));
%! assert (isequal (subs (f, 0), 1));
%! assert (isnan (subs (f, 1)));

%!test
%! % heaviside
%! syms x real
%! f = rewrite (heaviside (x, 1 / sym (2)), 'Piecewise');
%! g = piecewise (x < 0, 0, x == 0, 1 / sym (2), x > 0, 1);
%! assert (logical (simplify (f == g)));

%% FIXME: expand test suite, add SMT compat tests, ...


function out = iimg_reslice(matchto, reslicethis, varargin)
% :Usage:
% ::
%
%     out = iimg_reslice(matchto, reslicethis, varargin)
%
%     out = iimg_reslice(matchto, reslicethis, 'write', 'outname', 'myimg.img')
%
% ..
%    tor wager
%    nov. 06
% ..

    % default flags
    % interp = 2 is trilinear
    flags = struct('interp', 2, 'vox', NaN, 'bb', NaN, 'wrap', [0 0 0], 'preserve', 0);
    % spm options for interpolation and wrapping
    d  = [flags.interp*[1 1 1]' flags.wrap(:)];

    write_vol = 0;
    if any(strcmp(varargin, 'write'))
        write_vol = 1;
    end

    % get output image name
    outname = 'resliced_image.img';
    argno = find(strcmp(varargin, 'outname'));
    if argno
        outname = varargin{argno + 1};
    end

    % get info of image to match to
    volInfo = iimg_read_img(matchto);

    % grid in space of target image
    [x1, x2] = ndgrid(1:volInfo.dim(1), 1:volInfo.dim(2));

    out = zeros(volInfo.dim(1:3));

    for i = 1	% for each image
        % get info for image to reslice
        volInfo2 = spm_vol(reslicethis);
        %[volInfo2, reslicethis] = iimg_read_img(reslicethis);

        if write_vol
            VO = volInfo2;
            VO.fname   = outname;

            switch(spm('Ver'))
                case 'SPM2'
                    VO.dim     = [volInfo.dim(1:3) volInfo2.dim(4)];
                case {'SPM5', 'SPM8'}
                    VO.dt = volInfo.dt;
                    VO.private.dat.fname = outname;

                otherwise
                    error('Unknown SPM version "%s": neuroscientists of the future, fix me!', spm('Ver'));
            end

            VO.mat     = volInfo.mat;
            VO.descrip = 'iimg_reslice - resliced - trilinear';
        end

        % final affine matrix
        % mat is affine mtx of image
        % Target\Object maps target to object
        affinemat = inv(volInfo.mat\volInfo2.mat);

        C = spm_bsplinc(volInfo2, d);

        % for each slice

        for x3 = 1:volInfo.dim(3)
            [tmp, y1, y2, y3] = getmask(affinemat, x1, x2, x3, volInfo2.dim(1:3), flags.wrap);
            out(:, :, x3)              = spm_bsplins(C, y1, y2, y3, d);
        end

        if write_vol
            spm_write_vol(VO, out);
        end
    end
end

function [Mask, y1, y2, y3] = getmask(M, x1, x2, x3, dim, wrp)
    tiny = 5e-2; % From spm_vol_utils.c
    y1   = M(1, 1)*x1+M(1, 2)*x2+(M(1, 3)*x3+M(1, 4));
    y2   = M(2, 1)*x1+M(2, 2)*x2+(M(2, 3)*x3+M(2, 4));
    y3   = M(3, 1)*x1+M(3, 2)*x2+(M(3, 3)*x3+M(3, 4));
    Mask = logical(ones(size(y1)));
    if ~wrp(1), Mask = Mask & (y1 >= (1-tiny) & y1 <= (dim(1)+tiny)); end;
    if ~wrp(2), Mask = Mask & (y2 >= (1-tiny) & y2 <= (dim(2)+tiny)); end;
    if ~wrp(3), Mask = Mask & (y3 >= (1-tiny) & y3 <= (dim(3)+tiny)); end;
end


function K = lfmXlfmKernCompute(lfmKern1, lfmKern2, t1, t2)

% LFMXLFMKERNCOMPUTE Compute a cross kernel between two LFM kernels.
% FORMAT
% DESC computes cross kernel terms between two LFM kernels for
% the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel.
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two LFM kernels for
% the multiple output kernel.
% ARG lfmKern1 : the kernel structure associated with the first LFM
% kernel.
% ARG lfmKern2 : the kernel structure associated with the second LFM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : lfmKernParamInit, lfmKernCompute, lfmKernParamInit
%
% COPYRIGHT : David Luengo, 2007, 2008, Mauricio Alvarez, 2008
%
% MODIFICATIONS : Neil D. Lawrence, 2007, 2008,
%

% KERN

if nargin < 4
    t2 = t1;
end
if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end
if lfmKern1.inverseWidth ~= lfmKern2.inverseWidth
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Get length scale out.
sigma2 = 2/lfmKern1.inverseWidth;
sigma = sqrt(sigma2);

% Parameters of the kernel
alpha(1) = lfmKern1.damper./(2*lfmKern1.mass);
alpha(2) = lfmKern2.damper./(2*lfmKern2.mass);
omega(1) = sqrt(lfmKern1.spring./lfmKern1.mass - alpha(1)*alpha(1));
omega(2) = sqrt(lfmKern2.spring./lfmKern2.mass - alpha(2)*alpha(2));

% Creation of the time matrices

if isreal(omega)
    % Precomputations to increase speed
    gamma1 = alpha(1) + j*omega(1);
    gamma2 = alpha(2) + j*omega(2);
    preGamma(1) = gamma1 + gamma2;
    preGamma(2) = conj(gamma1) + gamma2;
    preConst = 1./preGamma;
    preExp1 = exp(-gamma1*t1);
    preExp2 = exp(-gamma2*t2);
    % Actual computation of the kernel
    sK = real(lfmComputeH3(gamma1, gamma2, sigma2, t1,t2,preConst, 0, 1) + ...
        lfmComputeH3(gamma2, gamma1, sigma2, t2,t1,preConst(2) - preConst(1), 0, 0).' + ...
        lfmComputeH4(gamma1, gamma2, sigma2, t1, preGamma, preExp2, 0, 1  ) + ...
        lfmComputeH4(gamma2, gamma1, sigma2, t2, preGamma, preExp1,0, 0 ).');
    if lfmKern1.isNormalised
        K0 = (lfmKern1.sensitivity*lfmKern2.sensitivity)/(4*sqrt(2)*lfmKern1.mass*lfmKern2.mass*prod(omega));
    else
        K0 = (sigma*sqrt(pi)*lfmKern1.sensitivity*lfmKern2.sensitivity)/(4*lfmKern1.mass*lfmKern2.mass*prod(omega));
    end
    K = K0*sK;
else
    % Precomputations to increase the speed
    preExp1 = zeros(length(t1),2);
    preExp2 = zeros(length(t2),2);
    gamma1_p = alpha(1) + j*omega(1);
    gamma1_m = alpha(1) - j*omega(1);
    gamma2_p = alpha(2) + j*omega(2);
    gamma2_m = alpha(2) - j*omega(2);
    preGamma(1) = gamma1_p + gamma2_p;
    preGamma(2) = gamma1_p + gamma2_m;
    preGamma(3) = gamma1_m + gamma2_p;
    preGamma(4) = gamma1_m + gamma2_m;
    preConst = 1./preGamma;
    preFactors(1) = preConst(2) - preConst(1);
    preFactors(2) = preConst(3) - preConst(4);
    preFactors(3) = preConst(3) - preConst(1);
    preFactors(4) = preConst(2) - preConst(4);
    preExp1(:,1) = exp(-gamma1_p*t1);
    preExp1(:,2) = exp(-gamma1_m*t1);
    preExp2(:,1) = exp(-gamma2_p*t2);
    preExp2(:,2) = exp(-gamma2_m*t2);
    % Actual computation of the kernel
    sK = (  lfmComputeH3(gamma1_p, gamma1_m, sigma2, t1,t2,preFactors([1 2]), 1) + ...
        lfmComputeH3(gamma2_p, gamma2_m, sigma2, t2,t1,preFactors([3 4]), 1).' + ...
        lfmComputeH4(gamma1_p, gamma1_m, sigma2, t1, preGamma([1 2 4 3]), preExp2, 1 ) + ...
        lfmComputeH4(gamma2_p, gamma2_m, sigma2, t2, preGamma([1 3 4 2]), preExp1, 1 ).');
    if lfmKern1.isNormalised
        K0 = (lfmKern1.sensitivity*lfmKern2.sensitivity)/(8*sqrt(2)*lfmKern1.mass*lfmKern2.mass*prod(omega));
    else
        K0 = (sigma*sqrt(pi)*lfmKern1.sensitivity*lfmKern2.sensitivity)/(8*lfmKern1.mass*lfmKern2.mass*prod(omega));
    end
    K = K0*sK;
end


function [XYZworld,faces, RGBcolor, tsdf_value, tsdf_weight] = TSDFmeshSupposedToBeFasterButNot(data, frameIdtarget,frameIds,cameraRtC2W, is_color)

if ~exist('is_color','var')
    is_color = true;
end

cameraRtC2Wframes = cameraRtC2W(:,:,frameIds);
frameIdtargetID = find(frameIds==frameIdtarget);

mapRt = transformCameraRt(cameraRtC2Wframes(:,:,frameIdtargetID));

for i=1:length(frameIds)
    cameraRtC2Wframes(:,:,i) = concatenateRts(mapRt, cameraRtC2Wframes(:,:,i));
end

%% tsdf

voxel.unit = 0.01; % Kevin: 4mm = 0.004 meter. Kinect cannot go better than 3mm
voxel.mu_grid = 10; % used to be 4
voxel.size_grid = [512; 512; 1024]; % [512; 512; 512];

voxel.range(1,1) = - voxel.size_grid(1) * voxel.unit / 2;
voxel.range(1,2) = voxel.range(1,1) + (voxel.size_grid(1)-1) * voxel.unit;

voxel.range(2,1) = - voxel.size_grid(2) * voxel.unit / 2;
voxel.range(2,2) = voxel.range(2,1) + (voxel.size_grid(2)-1) * voxel.unit;

voxel.range(3,1) = -0.5; % - voxel.size_grid(3) * voxel.unit / 2;
voxel.range(3,2) = voxel.range(3,1) + (voxel.size_grid(3)-1) * voxel.unit;

voxel.mu = voxel.mu_grid * voxel.unit;

fprintf('memory = %f GB\n',  prod(voxel.size_grid) * 4 / (1024*1024*1024));
fprintf('space = %.2f m x %.2f m x %.2f m ', voxel.size_grid(1) * voxel.unit, voxel.size_grid(2) * voxel.unit, voxel.size_grid(3) * voxel.unit);
fprintf('= [%.2f,%.2f] x [%.2f,%.2f] x [%.2f,%.2f]\n',voxel.range(1,1),voxel.range(1,2),voxel.range(2,1),voxel.range(2,2),voxel.range(3,1),voxel.range(3,2));

%tsdf_value  = -ones([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');
tsdf_value  =  ones([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');
tsdf_weight = zeros([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)],'single');

if is_color
    tsdf_color  = zeros([voxel.size_grid(1),voxel.size_grid(2), voxel.size_grid(3)], 'uint32');
end

f = data.K(1,1);

ViewFrustumC = [...
    0 -data.K(1,3) -data.K(1,3)  data.K(1,3)  data.K(1,3);
    0 -data.K(2,3)  data.K(2,3)  data.K(2,3) -data.K(2,3);
    0    f    f    f    f];
ViewFrustumC = ViewFrustumC/f * 8; % 8 meter is the furthest depth of kinect

% precompute
[pX,pY]=meshgrid(1:640,1:480);

raycastingDirectionC = [pX(:)'-data.K(1,3); pY(:)'-data.K(2,3); f*ones(1,640*480)]; % clipping at 8 meter is the furthest depth of kinect
raycastingDirectionC = raycastingDirectionC ./ repmat(sqrt(sum(raycastingDirectionC.^2,1)),3,1);

%% get the grid there

underConsideration = true(1,prod(voxel.size_grid));

%% speed up by eliminate free space

% if you want to debug fast, enough some frames only
firstFrame = true;
%for i=1:33:length(frameIds)
for i=1:length(frameIds)

    fprintf('======================================== frame %d ========================================\n',i);

    camRtC2W = cameraRtC2Wframes(:,:,i);

    IMGcam = readImage(data.image{frameIds(i)});
    XYZcam = depth2XYZcamera(data.K, depthRead(data.depth{frameIds(i)}));

    ViewFrustumW = transformRT(ViewFrustumC, camRtC2W);

    range2test = [min(ViewFrustumW, [], 2) max(ViewFrustumW, [], 2)];

    % choose a bounding box to contain viewing frustum
    rangeGrid = (range2test - voxel.range(:,[1 1])) / voxel.unit + 1;
    rangeGrid(:,1) = max(1,floor(rangeGrid(:,1)));
    rangeGrid(:,2) = min(ceil (rangeGrid(:,2)),voxel.size_grid);
    rangeGrid = int32(rangeGrid);

    % get the grid there
    disp('meshgrid');
    tic;
    [Y,X,Z]=meshgrid(rangeGrid(1,1):rangeGrid(1,2),rangeGrid(2,1):rangeGrid(2,2),rangeGrid(3,1):rangeGrid(3,2)); % strange matlab syntax
    toc;
    disp('index generation');
    tic;
    X = X(:)'; Y = Y(:)'; Z = Z(:)';
    gridIndex = sub2ind(voxel.size_grid',X,Y,Z);
    toc;

    % speed up
    if firstFrame
        firstFrame = false;
    else
        disp('select voxels worth consideration');
        tic;
        underConsiderationHere = underConsideration(gridIndex);
        X = X(underConsiderationHere);
        Y = Y(underConsiderationHere);
        Z = Z(underConsiderationHere);
        gridIndex = gridIndex(underConsiderationHere);
        toc;
    end

    fprintf('%d voxels to consider\n',length(gridIndex));

    disp('world coordinate');
    tic;
    gridCoordinateW = [single(X)*voxel.unit + voxel.range(1,1); single(Y)*voxel.unit + voxel.range(2,1); single(Z)*voxel.unit + voxel.range(3,1)];
    clear X Y Z;
    toc;

    % transform the grid
    disp('transform');
    tic;
    gridCoordinateC = transformRT(gridCoordinateW, camRtC2W, true);
    toc;

    % select: in front of camera
    disp('select: in front of camera');
    tic;
    isValid = gridCoordinateC(3,:)>0; % find(gridCoordinateC(3,:)>0);
    gridCoordinateC = gridCoordinateC(:,isValid);
    gridIndex = gridIndex(isValid);
    toc;

    % select: project
    disp('select: project');
    tic;
    px = round(data.K(1,1)*(gridCoordinateC(1,:)./gridCoordinateC(3,:)) + data.K(1,3));
    py = round(data.K(2,2)*(gridCoordinateC(2,:)./gridCoordinateC(3,:)) + data.K(2,3));
    isValid = (1<=px & px <= 640 & 1<=py & py<= 480);
    gridCoordinateC = gridCoordinateC(:,isValid);
    gridIndex = gridIndex(isValid);
    py = py(isValid);
    px = px(isValid);
    toc;

    % select: valid depth
    disp('select: valid depth');
    tic;
    ind = sub2ind([480 640],py,px);
    isValid = XYZcam(ind+640*480*3)~=0;
    gridCoordinateC = gridCoordinateC(:,isValid);
    gridIndex = gridIndex(isValid);
    ind = ind(isValid);
    toc;

    % compare distance between measurement and the grid
    disp('compare distance between measurement and the grid');
    %{

            const float diff = (depth[px] - cameraX.z) * sqrt(1+sq(pos.x/pos.z) + sq(pos.y/pos.z));
            if(diff > -mu){
                const float sdf = fminf(1.f, diff/mu);
                float2 data = vol[pix];
                data.x = clamp((data.y*data.x + sdf)/(data.y + 1), -1.f, 1.f);
                data.y = fminf(data.y+1, maxweight);
                vol.set(pix, data);
            }

    %}
    tic;
    eta = (XYZcam(ind+640*480*2)- gridCoordinateC(3,:)) .* ((1+ (gridCoordinateC(1,:)./gridCoordinateC(3,:)).^2 + (gridCoordinateC(2,:)./gridCoordinateC(3,:)).^2 ).^0.5);
    toc;

    %tic;
    %XYZmeasure = [XYZcam(ind);XYZcam(ind+640*480);XYZcam(ind+640*480*2)];
    %Dmeasure = sqrt(sum(XYZmeasure.^2,1));
    %Dtran    = sqrt(sum(gridCoordinateC.^2,1));
    %eta = Dmeasure-Dtran;
    %toc;

    %
    %disp('speed up');
    %isValid = eta>voxel.mu*2;
    %underConsideration(gridIndex(isValid)) = false;

    % select: > - mu
    disp('select: > - mu');
    tic;
    isValid = eta>-voxel.mu;
    eta = eta(isValid);
    gridIndex = gridIndex(isValid);
    ind = ind(isValid);
    toc;

    new_value = min(1,eta/voxel.mu);

    disp('read write tsdf');
    tic;
    old_weight = tsdf_weight(gridIndex);
    new_weight = old_weight + 1;
    tsdf_weight (gridIndex)= new_weight;
    tsdf_value (gridIndex) = (tsdf_value(gridIndex).*old_weight +new_value)./new_weight;
    toc;

    % update speed up vector
    disp('updating speed up vector'); tic;
    underConsideration(gridIndex(eta>voxel.mu*2)) = false;
    toc;

    if is_color
        disp('coloring');
        tic;
        isValid = eta<=voxel.mu;
        eta = eta(isValid);
        gridIndex = gridIndex(isValid);
        ind = ind(isValid);

        IMGmeasure = [IMGcam(ind);IMGcam(ind+640*480);IMGcam(ind+640*480*2)]*255;

        colorArray = reshape(typecast(tsdf_color(gridIndex),'uint8'),4,[]);

        old_weight = colorArray(4,:);
        old_weight(double(old_weight)>=255) = 254;
        new_weight = old_weight + 1;

        newArray = uint8(round((double(colorArray(1:3,:)) .* repmat(double(old_weight),3,1) + double(IMGmeasure)) ./ repmat(double(new_weight),3,1)));
        newArray(4,:) = new_weight;

        tsdf_color(gridIndex) = typecast(newArray(:),'uint32');
        toc;
    end

    % visualizing the voxel
    %{
    figure(1)
    min_weight = min(tsdf_weight(:));
    max_weight = max(tsdf_weight(:));
    for ii=1:voxel.size_grid(3)
        if min(min(min(tsdf_value(:,:,ii)))) ~= max(max(max(tsdf_value(:,:,ii))))
            imagesc(tsdf_weight(:,:,ii)',[min_weight max_weight]); axis equal; axis tight; xlabel('x'); ylabel('y');
            title(['frame ' num2str(ii) ' = depth ' num2str((ii-1)*voxel.unit+voxel.range(3,1)) ' meter']);
            pause(0.05);
        end
    end

    figure(2)
    for ii=1:voxel.size_grid(3)
        if min(min(min(tsdf_value(:,:,ii)))) ~= max(max(max(tsdf_value(:,:,ii))))
            imagesc(tsdf_value(:,:,ii)',[-1 1]); axis equal; axis tight; xlabel('x'); ylabel('y');
            title(['frame ' num2str(ii) ' = depth ' num2str((ii-1)*voxel.unit+voxel.range(3,1)) ' meter']);
            pause(0.05);
        end
    end
    %}
end

%% meshing
disp('isosurfacing...');
tic;
fv = isosurface(tsdf_value,0);
toc;
% visualizaiton
%{
figure(3);
p = patch(fv);
p.FaceColor = 'red';
p.EdgeColor = 'none';
daspect([1,1,1])
view(3); axis tight
camlight
lighting gouraud
%}

%% remove bad points that are artifact of TSDF

XYZ = fv.vertices';

faces = fv.faces';

XYZ = XYZ([2 1 3],:);

XYZint = round(XYZ);

selected = 1< XYZint(1,:) & XYZint(1,:) < voxel.size_grid(1) & 1< XYZint(2,:) & XYZint(2,:) < voxel.size_grid(2) & 1< XYZint(3,:) & XYZint(3,:) < voxel.size_grid(3);
XYZint = XYZint(:,selected);
XYZ = XYZ(:,selected);
faces = faces(:,selected(faces(1,:)) & selected(faces(2,:)) & selected(faces(3,:)));
indNew = cumsum(double(selected));
faces = indNew(faces);

gridIndex = sub2ind(voxel.size_grid',XYZint(1,:),XYZint(2,:),XYZint(3,:));
valIndex = tsdf_value(gridIndex);
selected = -0.15 < valIndex & valIndex<0.15;
XYZint = XYZint(:,selected);
XYZ = XYZ(:,selected);
faces = faces(:,selected(faces(1,:)) & selected(faces(2,:)) & selected(faces(3,:)));
indNew = cumsum(double(selected));
faces = indNew(faces);
XYZworld = (XYZ-1) *  voxel.unit + repmat(voxel.range(:,1),1,size(XYZ,2));

%% coloring
if is_color
    RGBcolor = zeros(3,size(XYZworld,2));
    disp('');
    tic
    for i=1:size(XYZworld,2)
        RGBcolor(:,i) = interpolateTrilinearyColor(XYZ(1,i),XYZ(2,i),XYZ(3,i),tsdf_color);
    end
    toc;
else
    RGBcolor = [];
end

% visualizaiton

figure(4)
clf;
plot3(XYZworld(1,:),XYZworld(2,:),XYZworld(3,:),'.r');
XYZcam = depth2XYZcamera(data.K, depthRead(data.depth{frameIdtarget}));
XYZcam = reshape(XYZcam,[],4)';
XYZcam = XYZcam(:,XYZcam(4,:)~=0);
XYZcam = XYZcam(1:3,:);
hold on
plot3(XYZcam(1,:),XYZcam(2,:),XYZcam(3,:),'.b');
axis equal;
axis tight;
grid on;
xlabel('x');
ylabel('y');
zlabel('z');

figure(5);
clf
fvNew.vertices = XYZworld';
fvNew.faces = faces';
%fvNew.faces = fvNew.faces(randsample(size(fvNew.faces,1),10000),:);
p = patch(fvNew,'FaceColor','none','EdgeColor','red');
daspect([1,1,1])
view(3); axis tight


%% patchSmoothFaceMeasure
% Below is a demonstration of the features of the |patchSmoothFaceMeasure| function

%% Syntax
% |[C_smooth]=patchSmoothFaceMeasure(F,V,C,smoothPar);|

%% Description
%

%% Examples

%%
clear; close all; clc;

%%
% Plot settings
markerSize=50;

%% Demonstrating

smoothPar.lambda=0.5; %Lambda for smoothing
smoothPar.n=25; %Number of smooth iterations

for testCase=1:6
    [F,V]=graphicsModels(testCase);
    switch testCase
        case 1 %Cut patch data with unused points
            [F,V]=geoSphere(4,1);
            Z=V(:,3);
            F=F(mean(Z(F),2)>0,:);
        case 2 %Cut patch data
            [F,V]=geoSphere(4,1);
            Z=V(:,3);
            F=F(mean(Z(F),2)>0,:);
            [F,V]=patchCleanUnused(F,V);
        case 3 %Quadrilateral patch data
            [F,V]=quadSphere(4,1);
        case 4 %A larger mesh
            [F,V]=graphicsModels(1);
        case 5 %A tangled mesh
            [F,V]=graphicsModels(8);
        case 6
            r=1; %Sphere radius
            rc=2.5; %Central radius
            nr=16;
            nc=25;
            patchType='honey';
            [F,V]=patchTorus(r,nr,rc,nc,patchType);
    end

    %%
    Z=V(:,3);
    C_clean=mean(Z(F),2);
    C=C_clean+(max(C_clean)-min(C_clean))/5*randn(size(F,1),1);

    %%
    [C_smooth]=patchSmoothFaceMeasure(F,V,C,smoothPar);

    %%
    cFigure;
    subplot(1,2,1); hold on;
    title('Raw face data');
    gpatch(F,V,C,'none');
    colorbar(gca); caxis([min(C_clean) max(C_clean)]);
    axisGeom;
    camlight headlight;

    subplot(1,2,2); hold on;
    title('Smoothed face data');
    gpatch(F,V,C_smooth,'none');
    colorbar(gca); caxis([min(C_clean) max(C_clean)]);
    axisGeom;
    camlight headlight;

    drawnow;

end

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


function [model, partition_size] = fit_partitioned_model(...
    inputs, outputs, selectors, sel_sizes, min_size, partition_names, fn_name, varargin)
%function [models, partition_sizes] = fit_partitioned_model(...
%    inputs, outputs, selectors, sel_sizes, min_size, partition_names, fn_name, varargin)
%
% Fit models to different subsets (columns)  of the input/output data,
% as chosen by the selectors matrix. If there is only output data, set input=[].
% If there is less than min_size data in partition i,
% we set model{i} = []
%
% Example:
% selectors = [1 2 1 1 1
%              1 2 2 1 2]
% sel_sizes = [2 2] so there are 4 models: (1,1), (2,1), (1,2), (2,2)
% We fit model{1} to data from columns 1,4
% We fit model{2} to no data
% We fit model{3} to data from column 3,5
% We fit model{4} to data from column 2 (assuming min_size <= 1)
%
% For each partition, we call the specified function with the specified arguments
% as follows:
%  model{i}  = fn(input(:,cols{i}), output(:,cols{i}), args)
% (We omit input if [])
% partition_size(i) is the amount of data in the i'th partition.
%
% Example use: row 1 of selectors is whether an object is present/absent
% and row 2 is the location.
%
% Demo:
% inputs = 1:5; outputs = 6:10; selectors = as above
% fn = 'fit_partitioned_model_testfn';
% [model, partition_size] = fit_partitioned_model(inputs, outputs, selectors, [2 2], fn)
% should produce
% model{1}.input = [1 4], model{1}.output = [6 9]
% model{2} = []
% model{3}.input = [3 5], model{3}.output = [8 10],
% model{4}.input = [2], model{3}.output = [7],
% partition_size = [2 0 2 1]

sel_ndx = subv2ind(sel_sizes, selectors');
Nmodels = prod(sel_sizes);
model = cell(1, Nmodels);
partition_size = zeros(1, Nmodels);
for m=1:Nmodels
  ndx = find(sel_ndx==m);
  partition_size(m) = length(ndx);
  if ~isempty(partition_names) % & (partition_size(m) < min_size)
    fprintf('partition %s has size %d, min size = %d\n', ...
	    partition_names{m}, partition_size(m), min_size);
  end
  if partition_size(m) >= min_size
    if isempty(inputs)
      model{m} = feval(fn_name, outputs(:, ndx), varargin{:});
    else
      model{m} = feval(fn_name, inputs(:,ndx), outputs(:, ndx), varargin{:});
    end
  end
end


function varargout=mod(varargin)
%MOD (overloaded)

switch class(varargin{1})

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        if ~isa(varargin{2},'double')
            error('MOD is currently only supported for DOUBLE second argument');
        end

        x = varargin{1};
        y = varargin{2};
        % Some boring code for scalarization
        if prod(size(x))==1 & prod(size(y))>1
            x = x*ones(size(y));
        elseif prod(size(y))==1 & prod(size(x))>1
            y = y*ones(size(x));
        end
        if ~all(size(x) == size(y))
            error('Matrix dimensions must agree.');
        end
        dim = size(x);
        x = reshape(x,prod(dim),1);
        y = reshape(y,prod(dim),1);
        z = [];
        % Create one variable for each element
        for i = 1:length(x)
            xi = extsubsref(x,i);
            yi = extsubsref(y,i);
            inarg = {xi,yi};
            z = [z;yalmip('define',mfilename,inarg{:})];
        end
        z = reshape(z,dim);
        varargout{1} = z;

    case 'char' % YALMIP send 'graph' when it wants the epigraph or hypograph

        % Description using epigraphs
        t = varargin{2};
        x = varargin{3};
        y = varargin{4};

        % t = mod(x,y), i.e. t = x - n*y, n = floor(x/y)
        n = intvar(1,1);
        [M,m] = derivebounds(x);
        F_floor = [(x/y) - 1 <= n <= (x/y)];
        if ~isinf(m) && isa(y,'double')
            F_floor = [F_floor, floor(m/y) <= n];
        end
        if ~isinf(M) && isa(y,'double')
            F_floor = [F_floor, n <= floor(M/y)];
        end
        F = [t == x - y*n, F_floor];

        varargout{1} = F;
        varargout{2} = struct('convexity','none','monotonicity','none','definiteness','none','model','integer');
        varargout{3} = [x(:);y(:)];

    otherwise
        error('Strange type on first argument in SDPVAR/MOD');
end


function geodemo_4b
%% GEODEMO_4b
% Compare horizontal slices from two different CF compliant structured
% grid models (EFDC and CBOFS) at a particular time step and depth, using
% geosubset to subset the data

%% EFDC
url{2}='http://comt.sura.org/thredds/dodsC/data/comt_1_archive/estuarine_hypoxia/VIMS_EFDC/2004_DO3d';
var{2}='salt';
titl{2}='EFDC';

%% ROMS
url{1}='http://comt.sura.org/thredds/dodsC/data/comt_1_archive/estuarine_hypoxia/VIMS_CBOFS/2004-2005';
var{1}='salt';
titl{1}='CBOFS';

%% Create geosubset object
dat=[2004 4 10 6 0 0];  % Apr 4, 2004 06:00 UTC
depth=-5;  % horizontal slice 5 m from surface
ax=[ -76.5220  -75.7105   36.8248   37.7850]; %lon/lat range
cax=[0 33];  %color range
lat_mid=38; % for scaling plots

s.time=datenum(dat);
s.lon=ax(1:2);
s.lat=ax(3:4);

%% Perform analysis without using dataset dependant code
% Access datasets, subset data, interpolate data to a constant z, plot results at z depth

figure;
for i=1:length(url);
  nc{i}=ncgeodataset(url{i});
  % create a salinity geovariable object.  No data read yet.
  svar{i}=geovariable(nc{i},var{i});
  disp(['reading data from ' titl{i} '...'])
  % using geosubset here, which allows for subsetting and striding, reading
  % multiple time steps, only certain zlevels and more.
  sub{i}=svar{i}.geosubset(s);
  sz{i}=zsliceg(squeeze(sub{i}.data),squeeze(sub{i}.grid.z),depth);
  a{i}=subplot(1,length(url),i);
  pcolorjw(sub{i}.grid.lon,sub{i}.grid.lat,double(sz{i}));colorbar
  axis(ax);
  caxis(cax);
  title(sprintf('%s, depth=%f: %s',titl{i},depth,datestr(sub{1}.grid.time)));
  set (a{i}, 'DataAspectRatio', [1 cos(lat_mid*pi/180) 1000] );
end


function [IND INT]=imsphere_IND_INT(r,v_low,res,size_IM,Xp,Yp,Zp)

% function [I J K]=imsphere_IND_INT(r,v_low,res,size_IM,Xp,Yp,Zp)
% ------------------------------------------------------------------------
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 07/08/2008
% ------------------------------------------------------------------------

%% Setting up voxel dimensions and voxel radius dimensions
v_high=v_low/res;
r_v_high=r/v_high(1);
r_v_low=r/v_low(1);

%% Calculating image coordinate of point in high resolution image
[Ip_high Jp_high Kp_high]=cart2im(Xp,Yp,Zp,v_high);

%% Finding the index of the voxel in which point is found or is closest to
Ir_high=round(Ip_high); Jr_high=round(Jp_high); Kr_high=round(Kp_high);

%% Calculating shift from the centre of this voxel
Ic_high=Ir_high-Ip_high; Jc_high=Jr_high-Jp_high; Kc_high=Kr_high-Kp_high;

%% Finding indices of points in a sphere at specified location
[I_sph_high J_sph_high K_sph_high]=sphere_index(r_v_high,Ic_high,Jc_high,Kc_high);

%Calculating image coordinate of point in low resolution image
[Ip_low Jp_low Kp_low]=cart2im(Xp,Yp,Zp,v_low);
%Finding the index of the voxel in which point is found
Ir_low=round(Ip_low); Jr_low=round(Jp_low); Kr_low=round(Kp_low);
%Calculating shift from the centre of this voxel
Ic_low=Ir_low-Ip_low; Jc_low=Jr_low-Jp_low; Kc_low=Kr_low-Kp_low;

%Calculating shift
I_low_high_shift=((Ir_low*res)-(res/2)+0.5)-Ir_high;
J_low_high_shift=((Jr_low*res)-(res/2)+0.5)-Jr_high;
K_low_high_shift=((Kr_low*res)-(res/2)+0.5)-Kr_high;

size_M=(res+(2*round(r_v_low+1)*res))*ones(1,3);
M=ones(size_M);
IJK_middle=round(size_M/2);

IJK_voxel_center(1)=IJK_middle(1)-I_low_high_shift;
IJK_voxel_center(2)=IJK_middle(2)-J_low_high_shift;
IJK_voxel_center(3)=IJK_middle(3)-K_low_high_shift;
%round_IJK_voxel_center=round(IJK_voxel_center);

I_sph_high=round(I_sph_high+IJK_voxel_center(1));
J_sph_high=round(J_sph_high+IJK_voxel_center(2));
K_sph_high=round(K_sph_high+IJK_voxel_center(3));
IND = sub2ind(size_M,I_sph_high,J_sph_high,K_sph_high);
M(IND)=0;

%disp(['High resolution sphere simulated using ', num2str(numel(IND)), ' voxels']);

%Calculating low resolution image
[M]=voxelate(M,res);

IND=find(M<1);
INT=M(IND);
[I,J,K]=ind2sub(size(M),IND);
IJK_middle=round(size(M)/2);
I=I-IJK_middle(1); J=J-IJK_middle(2); K=K-IJK_middle(3);

I=I+Ir_low;
J=J+Jr_low;
K=K+Kr_low;

[IND]=sub2ind(size_IM,I,J,K);

%% END

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


% PlotNormScatt(x,Scatt)
% Color plot of a normalized scattering representation
% x gives the horizontal axis
% nScatt is the normalized scattering tranform which is piecewise constant
% The color on each interval depends upon the path length:
% 0 is yellow, 1 is red, 2 is green 3 is blue, 4 is magenta

function plot_diracnorm_scat(x,nScatt)
	color(1) = 'y';
	color(2) = 'r';
	color(3) = 'g';
	color(4) = 'b';
	color(5) = 'm';
	color(6) = 'k';
	color(7:99) = 'k';

	N=size(x,2);

	n=1;
	while(n<N)
		X(1) = x(n);
		Y(1) = nScatt(n);
		m = (round(10^(7) * Y(1) - 10 * floor(Y(1) * 10^(6))))+1;
		y = Y(1);
		while ((y == Y(1)) && (n < N))
			n = n+1;
			y = nScatt(n);
		end
		X(2) = x(n-1);
		Y(2) = Y(1);
		plot(X,Y,color(m));
		hold on;
		X(1) = X(2);
		Y(1) = y;
		plot(X,Y,color(m));
	end
	hold off;
end


function [post nlZ dnlZ] = infLaplace(hyp, mean, cov, lik, x, y, opt)

% Laplace approximation to the posterior Gaussian process.
% The function takes a specified covariance function (see covFunctions.m) and
% likelihood function (see likFunctions.m), and is designed to be used with
% gp.m. See also infMethods.m.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch 2013-05-02.
%
% See also INFMETHODS.M.

persistent last_alpha                                   % copy of the last alpha
if any(isnan(last_alpha)), last_alpha = zeros(size(last_alpha)); end   % prevent

if nargin<=6, opt = []; end                        % make opt variable available
if isfield(opt,'postL'), postL = opt.postL;        % recompute matrix L for post
else postL = true; end                                           % default value

inf = 'infLaplace';
n = size(x,1);
if isnumeric(cov),  K = cov;                    % use provided covariance matrix
else K = feval(cov{:},  hyp.cov,  x); end       % evaluate the covariance matrix
if isnumeric(mean), m = mean;                         % use provided mean vector
else m = feval(mean{:}, hyp.mean, x); end             % evaluate the mean vector
likfun = @(f) feval(lik{:},hyp.lik,y,f,[],inf);        % log likelihood function

if any(size(last_alpha)~=[n,1])     % find a good starting point for alpha and f
  alpha = zeros(n,1);                      % start at mean if sizes do not match
else
  alpha = last_alpha;                                             % try last one
  if Psi(alpha,m,K,likfun) > -sum(likfun(m))     % default f==m better => use it
    alpha = zeros(n,1);
  end
end

% switch between optimisation methods
alpha = irls(alpha, m,K,likfun, opt);                         % run optimisation

f = K*alpha+m;                                  % compute latent function values
last_alpha = alpha;                                     % remember for next call
[lp,dlp,d2lp,d3lp] = likfun(f); W = -d2lp; isWneg = any(W<0);
post.alpha = alpha;                            % return the posterior parameters
post.sW = sqrt(abs(W)).*sign(W);             % preserve sign in case of negative

% diagnose optimality
err = @(x,y) norm(x-y)/max([norm(x),norm(y),1]);   % we need to have alpha = dlp
% dev = err(alpha,dlp);  if dev>1e-4, warning('Not at optimum %1.2e.',dev), end

if postL || nargout>1
  if isWneg                  % switch between Cholesky and LU decomposition mode
    % For post.L = -inv(K+diag(1./W)), we us the non-default parametrisation.
    [ldA, iA, post.L] = logdetA(K,W);   % A=eye(n)+K*W is as safe as symmetric B
    nlZ = alpha'*(f-m)/2 - sum(lp) + ldA/2;
  else
    sW = post.sW; post.L = chol(eye(n)+sW*sW'.*K);                   % recompute
    nlZ = alpha'*(f-m)/2 + sum(log(diag(post.L))-lp);   % ..(f-m)/2 -lp +ln|B|/2
  end
end

if nargout>2                                           % do we want derivatives?
  dnlZ = hyp;                                   % allocate space for derivatives
  if isWneg                  % switch between Cholesky and LU decomposition mode
    Z = -post.L;                                                 % inv(K+inv(W))
    g = sum(iA.*K,2)/2; % deriv. of ln|B| wrt W; g = diag(inv(inv(K)+diag(W)))/2
  else
    Z = repmat(sW,1,n).*solve_chol(post.L,diag(sW)); %sW*inv(B)*sW=inv(K+inv(W))
    C = post.L'\(repmat(sW,1,n).*K);                     % deriv. of ln|B| wrt W
    g = (diag(K)-sum(C.^2,1)')/2;                    % g = diag(inv(inv(K)+W))/2
  end
  dfhat = g.*d3lp;  % deriv. of nlZ wrt. fhat: dfhat=diag(inv(inv(K)+W)).*d3lp/2
  for i=1:length(hyp.cov)                                    % covariance hypers
    dK = feval(cov{:}, hyp.cov, x, [], i);
    dnlZ.cov(i) = sum(sum(Z.*dK))/2 - alpha'*dK*alpha/2;         % explicit part
    b = dK*dlp;                            % b-K*(Z*b) = inv(eye(n)+K*diag(W))*b
    dnlZ.cov(i) = dnlZ.cov(i) - dfhat'*( b-K*(Z*b) );            % implicit part
  end
  for i=1:length(hyp.lik)                                    % likelihood hypers
    [lp_dhyp,dlp_dhyp,d2lp_dhyp] = feval(lik{:},hyp.lik,y,f,[],inf,i);
    dnlZ.lik(i) = -g'*d2lp_dhyp - sum(lp_dhyp);                  % explicit part
    b = K*dlp_dhyp;                        % b-K*(Z*b) = inv(eye(n)+K*diag(W))*b
    dnlZ.lik(i) = dnlZ.lik(i) - dfhat'*( b-K*(Z*b) );            % implicit part
  end
  for i=1:length(hyp.mean)                                         % mean hypers
    dm = feval(mean{:}, hyp.mean, x, i);
    dnlZ.mean(i) = -alpha'*dm;                                   % explicit part
    dnlZ.mean(i) = dnlZ.mean(i) - dfhat'*(dm-K*(Z*dm));          % implicit part
  end
end

% Evaluate criterion Psi(alpha) = alpha'*K*alpha + likfun(f), where
% f = K*alpha+m, and likfun(f) = feval(lik{:},hyp.lik,y,  f,  [],inf).
function [psi,dpsi,f,alpha,dlp,W] = Psi(alpha,m,K,likfun)
  f = K*alpha+m;
  [lp,dlp,d2lp] = likfun(f); W = -d2lp;
  psi = alpha'*(f-m)/2 - sum(lp);
  if nargout>1, dpsi = K*(alpha-dlp); end

% Run IRLS Newton algorithm to optimise Psi(alpha).
function alpha = irls(alpha, m,K,likfun, opt)
  if isfield(opt,'irls_maxit'), maxit = opt.irls_maxit; % max no of Newton steps
  else maxit = 20; end                                           % default value
  if isfield(opt,'irls_Wmin'),  Wmin = opt.irls_Wmin; % min likelihood curvature
  else Wmin = 0.0; end                                           % default value
  if isfield(opt,'irls_tol'),   tol = opt.irls_tol;     % stop Newton iterations
  else tol = 1e-6; end                                           % default value

  smin_line = 0; smax_line = 2;           % min/max line search steps size range
  nmax_line = 10;                          % maximum number of line search steps
  thr_line = 1e-4;                                       % line search threshold
  Psi_line = @(s,alpha,dalpha) Psi(alpha+s*dalpha, m,K,likfun);    % line search
  pars_line = {smin_line,smax_line,nmax_line,thr_line};  % line seach parameters
  search_line = @(alpha,dalpha) brentmin(pars_line{:},Psi_line,5,alpha,dalpha);

  f = K*alpha+m; [lp,dlp,d2lp] = likfun(f); W = -d2lp; n = size(K,1);
  Psi_new = Psi(alpha,m,K,likfun);
  Psi_old = Inf;  % make sure while loop starts by the largest old objective val
  it = 0;                          % this happens for the Student's t likelihood
  while Psi_old - Psi_new > tol && it<maxit                       % begin Newton
    Psi_old = Psi_new; it = it+1;
    % limit stepsize
    W = max(W,Wmin); % reduce step size by increasing curvature of problematic W
    sW = sqrt(W); L = chol(eye(n)+sW*sW'.*K);            % L'*L=B=eye(n)+sW*K*sW
    b = W.*(f-m) + dlp;
    dalpha = b - sW.*solve_chol(L,sW.*(K*b)) - alpha; % Newton dir + line search
    [s_line,Psi_new,n_line,dPsi_new,f,alpha,dlp,W] = search_line(alpha,dalpha);
  end                                                  % end Newton's iterations

% Compute the log determinant ldA and the inverse iA of a square nxn matrix
% A = eye(n) + K*diag(w) from its LU decomposition; for negative definite A, we
% return ldA = Inf. We also return mwiA = -diag(w)/A.
function [ldA,iA,mwiA] = logdetA(K,w)
  [m,n] = size(K); if m~=n, error('K has to be nxn'), end
  A = eye(n)+K.*repmat(w',n,1);
  [L,U,P] = lu(A); u = diag(U);           % compute LU decomposition, A = P'*L*U
  signU = prod(sign(u));                                             % sign of U
  detP = 1;                 % compute sign (and det) of the permutation matrix P
  p = P*(1:n)';
  for i=1:n                                                       % swap entries
    if i~=p(i), detP = -detP; j = find(p==i); p([i,j]) = p([j,i]); end
  end
  if signU~=detP  % log becomes complex for negative values, encoded by infinity
    ldA = Inf;
  else            % det(L) = 1 and U triangular => det(A) = det(P)*prod(diag(U))
    ldA = sum(log(abs(u)));
  end
  if nargout>1, iA = U\(L\P); end               % return the inverse if required
  if nargout>2, mwiA = -repmat(w,1,n).*iA; end


function [p] = loadPlant_QuadRotor3d()
%
% Convenience function to load 3d quadcopter.
% Provided to simplify main function and modularize plant model definition.
%
% Depends:
%   - definePropulsionModel.m

% Enviromental params
p.g = -9.81 ; % World Coords is XYZ = [East, North, Up], i.e. gravity is a negative number
p.rho = 1.225 ; % air density during flight (kg/m^3)

% Inertial params
p.m = 5 ;
p.I = [0.625 0 0; 0 0.625 0; 0 0 1.25] ; % inertia tensor coords:
p.cg = [0 0 0] ; % (m) location of center of gravity

% control params
p.uMax = 1 ; % maximum throttle setting

% Propulsion system params - shared for all motors:
qRP.d_prop = 0.305*ones(4,1) ; % propeller diameter (m)
qRP.maxThrust = 25*ones(4,1) ; % thrust at 100% throttle (N)
qRP.maxRPM = 10000*ones(4,1) ; % RPM at 100% throttle (RPM)
qRP.maxTorque = ones(4,1) ;  % torque at 100% throttle (Nm)
qRP.thrustLocations = [0.5 0 0; 0 0.5 0; -0.5 0 0; 0 -0.5 0]; % motor locations (each row one motor in coords: [port, nose, top]
qRP.thrustAxes = repmat([0 0 1],4,1) ; % thrust axes of each motor in coords port, nose, top.
qRP.isSpinDirectionCCW = [1; 0; 1; 0] ; % bool to reverse motor spin direction around 'thrustAxes'.
plotflag = 0 ;
[p.propulsion] = definePropulsionModel(qRP,plotflag);


function [fx,dfdx,dfdp] = f_HRF3(Xt,P,ut,in)
% Balloon (HRF) model evolution function in log-space
% function [fx,dfdx,dfdp] = f_HRF2(Xt,P,ut,in)
% This function evaluates the evolution function derived from the balloon
% model for the hemodynamic response function. It can be called in two
% ways: (i) as a "stand-alone" evolution function, whereby the system's
% states are hemodynamic states of the balloon model, or (ii) as a
% generalized observation function, where the real system's states are the
% neuronal states of a DCM for fMRI model. Note that the hemodynamic states
% are in log-space, for positivity constrints.

deltat = in.deltat;
n = size(Xt,1);

% Get parameters
[E0,V0,tau0,kaf,kas,epsilon,alpha] = BOLD_parameters;
nreg = numel(in.n5); %n./5;
ind1 = in.ind1;
ind2 = in.ind2;
ind3 = in.ind3;
ind4 = in.ind4;
n1 = in.n1;
n2 = in.n2;
n3 = in.n3;
n4 = in.n4;
n5 = in.n5;
epsilon = 1;
alpha = alpha.*exp(P(in.ind5));

E0 = 1./(1+exp(-(P(ind1)-0.6633)));
dsdp = E0.*(1-E0);
tau0 = tau0.*exp(P(ind2));
kaf = kaf.*exp(P(ind3));
kas = kas.*exp(P(ind4));

% hemodynamic states ...
if in.linearized
    x1 = zeros(nreg,1);
    x2 = ones(nreg,1);
    x3 = ones(nreg,1);
    x4 = ones(nreg,1);
else
    % vasodilatory signal s(t)
    x1 = Xt(n1,:);
    % blood inflow f(t)
    if ~in.logx2
        x2 = Xt(n2,:) + 1; % deviation to steady-state!
    else
        x2 = exp(Xt(n2,:)) + in.xshift;
    end
    % blood volume v(t)
    x3 = exp(Xt(n3,:)) + 0.*in.xshift;
    % dHb content q(t)
    x4 = exp(Xt(n4,:)) + 0.*in.xshift;
end
% blood outflow
fv = x3.^(1./alpha);
% d[blood flow]/dXt(3)
dfvdx = (1./alpha).*fv;
% oxygen extraction
ff = (1-(1-E0).^(1./x2))./E0;
% d[O2 extraction]/dXt(2)
if ~in.logx2
    dffdx = log(1-E0).*(1-E0).^(1./x2)./(E0.*x2.^2);
else
    dffdx = log(1-E0).*(1-E0).^(1./x2)./(E0.*x2);
end
% ... and flow field, derivatives, etc...
f = zeros(n,1);
J = zeros(n,n);
dfdp = zeros(size(P,1),n);

% Evaluate flow field
f(n1) = (epsilon.*ut - kas.*x1 - kaf.*(x2 - 1));
if ~in.logx2
    f(n2) = x1;
else
    f(n2) = x1./x2;
end
f(n3) = (x2 - fv)./(tau0.*x3);
f(n4) = (x2.*ff./x4 - fv./x3)./tau0;

% Evaluate jacobian and gradients wrt parameters
for i=1:nreg

    if ~in.logx2
        J(n1(i),n1(i):n1(i)+3) = [ -kas(i) , 1, 0, 0 ];
        J(n2(i),n1(i):n1(i)+3) = [ -kaf(i), 0, ...
            1./(tau0(i).*x3(i)), ...
            (ff(i)+x2(i).*dffdx(i))./(x4(i).*tau0(i))];
    else
        J(n1(i),n1(i):n1(i)+3) = [ -kas(i) , 1./x2(i), 0, 0 ];
        J(n2(i),n1(i):n1(i)+3) = [ -kaf(i).*x2(i), -x1(i)./x2(i), ...
            x2(i)./(tau0(i).*x3(i)), ...
            x2(i).*(ff(i)+dffdx(i))./(tau0(i).*x4(i))];
    end
    J(n3(i),n1(i):n1(i)+3) = [ 0, 0,...
        -f(n3(i)) - dfvdx(i)./(tau0(i).*x3(i)) , ...
        (fv(i)-dfvdx(i))./(tau0(i).*x3(i))];
    J(n4(i),n1(i):n1(i)+3) = [ 0, 0, 0, ...
        -(x2(i).*ff(i))./(tau0(i).*x4(i)) ];
    J(n5(i),n1(i)) = epsilon;

    if in.linearized

        tmp = log(1-E0(i))./E0(i);

        dfdp(ind1(i),n1(i):n1(i)+3) = [0,0,0,...
            -dsdp(i).*(1+tmp).*Xt(n2(i))./(tau0(i).*E0(i))];
        dfdp(ind2(i),n1(i):n1(i)+3) = [0,0,...
            (-Xt(n2(i))+Xt(n3(i))./alpha(i))./tau0(i),...
            -((ff(i)+dffdx(i)).*Xt(n2(i)) + (fv(i)-dfvdx(i)).*Xt(n3(i)) ...
            - ff(i).*Xt(n4(i)))./tau0(i)];
        dfdp(ind3(i),n1(i):n1(i)+3) = kaf(i).*[-Xt(n2(i)),0,0,0];
        dfdp(ind4(i),n1(i):n1(i)+3) = kas(i).*[-Xt(n1(i)),0,0,0];
        dfdp(in.ind5(i),n1(i):n1(i)+3) = [0,0,...
            Xt(n3(i))./(tau0(i).*alpha(i)),...
            Xt(n3(i))./(tau0(i).*alpha(i))];

    else

        % gradient wrt parameters
        dfdp(ind1(i),n1(i):n1(i)+3) = [0,0,0,...
            (((1-E0(i)).^(-1+1./x2(i)))-x2(i).*ff(i))...
            .*dsdp(i)./(tau0(i).*x4(i).*E0(i))];
        dfdp(ind2(i),n1(i):n1(i)+3) = [0,0,...
            -(x2(i) - fv(i))./(tau0(i).*x3(i)),...
            -(x2(i).*ff(i)./x4(i) - fv(i)./x3(i))./tau0(i)];
        dfdp(ind3(i),n1(i):n1(i)+3) = kaf(i).*[-x2(i)+1,0,0,0];
        dfdp(ind4(i),n1(i):n1(i)+3) = kas(i).*[-x1(i),0,0,0];
        dfdp(in.ind5(i),n1(i):n1(i)+3) = [0,0,...
            log(x3(i)).*fv(i)./(tau0(i).*x3(i).*alpha(i)),...
            log(x3(i)).*fv(i)./(tau0(i).*x3(i).*alpha(i))];

    end

end

% Apply Euler discretization
dfdp = deltat.*dfdp;
dfdx = eye(n) + deltat.*J;
if in.linearized
    fx = dfdx'*Xt;
else
    fx = Xt + deltat.*f;
end


function obj = JZMO_wbmethod(obj)

global V_ropt W_ropt Nk Nrf Ns n Vn H;
t1 = clock;
[V_RF, V_U] =  MO_AltMinWB(V_ropt);
[W_RF, W_B] =  MO_AltMinWB(W_ropt);
%W_B = W_B/20;
v = zeros(Nk,1);
V_B = zeros(Nrf,Ns,Nk);
for i = 1:Nk
    v(i) = trace(V_U(:,:,i)'*V_RF'*V_RF*V_U(:,:,i));
    V_B(:,:,i)= V_U(:,:,i) /sqrt(v(i));
end
for k = 1:Nk
    newH(:,:,k) = W_B(:,:,k)'*W_RF'*H(:,:,k)*V_RF*V_B(:,:,k);
    W_X(:,:,k) = inv(newH(:,:,k)*newH(:,:,k)' +  Vn * v(k) *W_B(:,:,k)'*(W_RF)'*W_RF*W_B(:,:,k))*newH(:,:,k);
    newnewH(:,:,k) = W_X(:,:,k)'*newH(:,:,k);
    W_B(:,:,k) = W_B(:,:,k) * W_X(:,:,k) ;
end
t2 = clock;
runtime  = etime(t2,t1);
obj.V_B = V_B;
obj.W_B = W_B;
obj.V_RF = V_RF;
obj.W_RF = W_RF;
obj.runtime = obj.runtime + runtime;
obj = get_wbmetric(obj);
% if (obj.ber(n)>30)
%     save H
%     pause
% end
a = 1;


function mM = minandmax2est(f, N)
%MINANDMAX2EST   Estimates the minimum and maximum of a SEPARABLEAPPROX.
%   mM = MINANDMAX2EST(F) returns estimates for the minimum and maximum of the
%   SEPARABLEAPPROX F over its domain.  mM is a vector of length 2 such that
%   mM(1) is the estimated minimum and mM(2) is the estimated maximum.
%
%   mM = MINANDMAX2EST(F, N) returns estimates for the minimum and maximum of
%   the SEPARABLEAPPROX F over its domain, based on samples on an N by N grid
%   (N = 33 by default).
%
% See also MINANDMAX2.

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

if ( isempty(f) )
    mM = [0, 0];
    return
end

if ( ( nargin < 2 ) || isempty(N) )
    % Default to N = 33:
    N = 33;
end

% Sample f on an appropriate grid:
vals = sample(f, N, N);

% Make result a column vector:
vals = vals(:);

% Get min and max:
mM = [ min(vals), max(vals) ];

end


function A = countTransitions(data, nstates)
%% Count the discrete transitions that occur in data
% data is a cell array of cases of potentially different lengths.
%%

% This file is from pmtk3.googlecode.com

if ~iscell(data)
   if isvector(data)
       data = {data};
   else
       data = mat2cellRows(data);
   end
end

A = zeros(nstates, nstates);
for i=1:numel(data)
   obs = colvec(data{i});
   A = A + accumarray([obs(1:end-1), obs(2:end)], 1, [nstates, nstates]);
end

end


filename='Cantileverbeam_Tetrahedra_Linear_Structured';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';
constraint_case = 'INEQUALITY';

nsteps = 10;
Vfrac_final = 0.2;
Perimeter_target=3.5;
optimality_final =1e-5;
constr_final =1e-5;

BCscale_factor = 0.3;
HJiter0 = 1;
e2 = 1;
N_holes = [6 3 3];
R_holes = 0.1;
phase_holes = [0 0 0];

Vfrac_initial = 0.8;
optimality_initial = 5e-2;
constr_initial = 5e-2;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

% maxiter = 10;
% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;

plotting = 1;
printing = 1;
monitoring = 1;
monitoring_interval = 1;