function pot = convert_to_pot(CPD, pot_type, domain, evidence)
% CONVERT_TO_POT Convert a tabular CPD to one or more potentials
% pot = convert_to_pot(CPD, pot_type, domain, evidence)

% This is the same as discrete_CPD/convert_to_pot,
% except we didn't want to the kernel to inherit methods like sample_node etc.

sz = CPD.sz;
ns = zeros(1, max(domain));
ns(domain) = sz;

odom = domain(~isemptycell(evidence(domain)));
T = convert_to_table(CPD, domain, evidence);

switch pot_type
 case 'u',
  pot = upot(domain, sz, T, 0*myones(sz));
 case 'd',
  ns(odom) = 1;
  pot = dpot(domain, ns(domain), T);
 case 'c',
  % Since we want the output to be a Gaussian, the whole family must be observed.
  % In other words, the potential is really just a constant.
  p = T.p;
  %p = prob_node(CPD, evidence(domain(end)), evidence(domain(1:end-1)));
  ns(domain) = 0;
  pot = cpot(domain, ns(domain), log(p));
 case 'cg',
  T = T(:);
  ns(odom) = 1;
  can = cell(1, length(T));
  for i=1:length(T)
    can{i} = cpot([], [], log(T(i)));
  end
  pot = cgpot(domain, [], ns, can);
end


function [ h_p ] = rl_2D_plot_policy(X,Y,Q,actions,dx,dy,h_p)
%RL_2D_PLOT_POLICY
%
%   input -----------------------------------------------------------------
%
%       o X       : meshgrid X values.
%
%       o Y       : meshgrid Y values
%
%       o Q       : (num_states x num_actions)
%
%       o actions : (M x 2)
%
%       o h_p     : plot function handle
%

[~,idx] = max(Q,[],2);
uv      = 0.5 .* actions(idx,:);

if isempty(h_p)
    h_p = quiver(X,Y,uv(:,1),uv(:,2),0,'Color',[0 0 0],'Linewidth',2);
else

    set(h_p,'UData',uv(:,1),'VData',uv(:,2));

end

end


classdef S2KernelHandle < S2Kernel
  % defines a kernel function as a function of the rotational angle

  properties
    fun = @(x) 1;
  end

  methods

    function psi = S2KernelHandle(fun,varargin)

      % extract parameter and halfwidth
      if nargin == 0, return;end

      psi.fun = fun;
      psi.A = calcFourier(psi,getMTEXpref('maxS2Bandwidth'),varargin{:});

    end

    function value = eval(psi,t)
      value = psi.fun(t);
    end

  end
end


classdef ConformalMappingComputer < handle

    properties (Access = private)
        phi
        interpolator
        phiMapping
        totalCorrector
    end

    properties (Access = private)
        orientationVector
        dilatedOrientation
        mesh
        isCoherent
    end

    methods (Access = public)

        function obj = ConformalMappingComputer(cParams)
            obj.init(cParams);
            obj.isOrientationCoherent();
            obj.createInterpolator();
        end

        function phiV = compute(obj)
            obj.computeMappingWithSingularities();
            phiV = obj.phi;
        end

        function plot(obj)
            for iDim = 1:obj.mesh.ndim
                obj.phi{iDim}.plotContour();
            end
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh               = cParams.mesh;
            obj.orientationVector  = cParams.orientationVector;
            obj.dilatedOrientation = cParams.dilatedOrientation;
        end

        function isOrientationCoherent(obj)
            s.mesh        = obj.mesh;
            s.orientation = obj.orientationVector{1}.project('P1D');
            c = CoherentOrientationSelector(s);
            isC = c.isOrientationCoherent();
            obj.isCoherent = isC;
        end

        function createInterpolator(obj)
            s.mesh       = obj.mesh;
            s.isCoherent = obj.isCoherent;
            s = SymmetricContMapCondition(s);
            sC = s.computeCondition();
            obj.interpolator = sC;
        end

        function computeMappings(obj)
            nDim = obj.mesh.ndim;
            for iDim = 1:nDim
                bI    = obj.dilatedOrientation{iDim};
                phiD  = obj.computeMapping(bI);
                phiV(iDim,:,:) = phiD.fValues;
            end
            s.fValues = phiV;
            s.mesh    = obj.mesh;
            phiF = P1DiscontinuousFunction(s);
            obj.phiMapping = phiF;
        end

        function computeTotalCorrector(obj)
           s.mesh = obj.mesh;
           s.isCoherent         = obj.isCoherent;
           s.dilatedOrientation = obj.dilatedOrientation;
           s.interpolator = obj.interpolator;
           s.phiMapping   = obj.phiMapping;
           tC = TotalCorrectorComputer(s);
           obj.totalCorrector = tC.compute();
        end

        function computeMappingWithSingularities(obj)
            obj.computeMappings();
            obj.computeTotalCorrector();
            psiTs = P1DiscontinuousFunction.sum(obj.phiMapping,obj.totalCorrector);
            obj.phi = psiTs;
        end

        function phi = computeMapping(obj,b)
            s.orientation  = b;
            s.mesh         = obj.mesh;
            s.interpolator = obj.interpolator;
            mC  = MappingComputer(s);
            phi = mC.compute();
        end

    end

end


function [L1t,L2t,L3t]=ongrid(V1,V2,V3,cm,vm)

L1t=ones(size(V1,1),size(cm,1));
L2t=ones(size(V2,1),size(cm,1));
L3t=ones(size(V3,1),size(cm,1));

for i=1:1:size(cm,1)

    c=cm(i,:);
    v=vm(i,:);

    [X1,Y1,Z1]=snap2grid(V1(:,1),V1(:,2),V1(:,3),c,v);
    X1=(X1-c(1))./v(1); Y1=(Y1-c(2))./v(2); Z1=(Z1-c(3))./v(3);
    V1s=[X1(:) Y1(:) Z1(:)];

    [X2,Y2,Z2]=snap2grid(V2(:,1),V2(:,2),V2(:,3),c,v);
    X2=(X2-c(1))./v(1); Y2=(Y2-c(2))./v(2); Z2=(Z2-c(3))./v(3);
    V2s=[X2(:) Y2(:) Z2(:)];

    [X3,Y3,Z3]=snap2grid(V3(:,1),V3(:,2),V3(:,3),c,v);
    X3=(X3-c(1))./v(1); Y3=(Y3-c(2))./v(2); Z3=(Z3-c(3))./v(3);
    V3s=[X3(:) Y3(:) Z3(:)];

    Vsnap=[V1s;V2s;V3s];
    Vsnap=1+(Vsnap-ones(size(Vsnap,1),1)*min(Vsnap,[],1));
    vsiz=max(Vsnap,[],1);
    Vsnap1=Vsnap(1:size(V1,1),:);
    Vsnap2=Vsnap(size(V1,1)+1:size(V1,1)+size(V2,1),:);
    Vsnap3=Vsnap(size(V1,1)+size(V2,1)+1:size(V1,1)+size(V2,1)+size(V3,1),:);

    INDsnap1=sub2ind(vsiz,Vsnap1(:,1),Vsnap1(:,2),Vsnap1(:,3));
    INDsnap2=sub2ind(vsiz,Vsnap2(:,1),Vsnap2(:,2),Vsnap2(:,3));
    INDsnap3=sub2ind(vsiz,Vsnap3(:,1),Vsnap3(:,2),Vsnap3(:,3));

    L1t(:,i)=ismembc(INDsnap1,sort(INDsnap2)) & ismembc(INDsnap1,sort(INDsnap3));
    L2t(:,i)=ismembc(INDsnap2,sort(INDsnap1)) & ismembc(INDsnap2,sort(INDsnap3));
    L3t(:,i)=ismembc(INDsnap3,sort(INDsnap1)) & ismembc(INDsnap3,sort(INDsnap2));

end

L1t=any(L1t,2);
L2t=any(L2t,2);
L3t=any(L3t,2);

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/>.


%%********************************************************************
%% infeaspt: generate an initial point for sdp.m
%%
%%  [X0,y0,Z0] = infeaspt(blk,At,C,b,options,scalefac);
%%
%%  options = 1  if want X0,Z0 to be scaled identity matrices
%%          = 2  if want X0,Z0 to be scalefac*(identity matrices).
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%********************************************************************

function [X0,y0,Z0] = infeaspt(blk,At,C,b,options,scalefac)
%%
if (nargin < 5); options = 1; end;
if (options == 1); scalefac = []; end;
if (options == 2) && (nargin < 6); scalefac = 1000; end;
if (scalefac <= 0); error('scalefac must a positive number'); end;
%%
if ~iscell(At); At = {At}; end;
if ~iscell(C);  C = {C}; end;
m = length(b);
if all(size(At) == [size(blk,1) m]);
    convertyes = zeros(size(blk,1),1);
    for p = 1:size(blk,1)
        if strcmp(blk{p,1},'s') && all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
        end
    end
    if any(convertyes)
        At = svec(blk,At,ones(size(blk,1),1));
    end
end;
%%
%%[blk,At,C,b] = validate(blk,At,C,b);
%%
X0 = cell(size(C)); Z0 = cell(size(C));
m = length(b);
for p = 1:size(blk,1);
    pblk = blk(p,:);
    blktmp = pblk{2};
    n = length(C{p});
    y0 = zeros(m,1);
    b2 = 1 + abs(b');
    if (options == 1);
        if strcmp(pblk{1},'s');
            normAni = [];
            X0{p} = sparse(n,n); Z0{p} = sparse(n,n);
            ss = [0, cumsum(blktmp)];
            tt = [0, cumsum(blktmp.*(blktmp+1)/2)];
            for i = 1:length(pblk{2})
                if ~isempty(At{p,1})
                    pos = tt(i)+1 : tt(i+1);
                    Ai = At{p,1}(pos,:);
                    normAni = 1+sqrt(sum(Ai.*Ai));
                end
                if (length(At(p,:)) >= 2) %% for low rank constraints
                    dd = At{p,3};
                    qq = [0, cumsum(pblk{3})]; normtmp = ones(1,length(pblk{3}));
                    idxD = [0; find(diff(dd(:,1))); size(dd,1)];
                    for k = 1:length(pblk{3})
                        idx = qq(k)+1 : qq(k+1);
                        idx2 = idxD(k)+1: idxD(k+1);
                        Ak = At{p,2}(:,idx);
                        ii = dd(idx2,2)-qq(k); %% undo cumulative indexing
                        jj = dd(idx2,3)-qq(k);
                        len = pblk{3}(k);
                        Dk = spconvert([ii,jj,dd(idx2,4); len,len,0]);
                        tmp = Ak'*Ak*Dk;
                        normtmp(1,k) = 1+sqrt(sum(sum(tmp.*tmp')));
                    end
                    normAni = [normAni, normtmp]; %#ok
                end
                pos = ss(i)+1 : ss(i+1);  ni = length(pos);
                tmp = C{p}(pos,pos);
                normCni = 1+sqrt(sum(sum(tmp.*tmp)));
                const  = 10; %%--- old: const = 1;
                constX = max([const,sqrt(ni),ni*(b2./normAni)]);
                constZ = max([const,sqrt(ni),normAni,normCni]);
                X0{p}(pos,pos) = constX*spdiags(1+1e-10*randmat(ni,1,0,'u'),0,ni,ni);
                Z0{p}(pos,pos) = constZ*spdiags(1+1e-10*randmat(ni,1,0,'u'),0,ni,ni);
            end
        elseif strcmp(pblk{1},'q');
            s = 1+[0, cumsum(blktmp)];
            len = length(blktmp);
            normC = 1+norm(C{p});
            normA = 1+sqrt(sum(At{p,1}.*At{p,1}));
            idenqX = zeros(sum(blktmp),1);
            idenqZ = zeros(sum(blktmp),1);
            idenqX(s(1:len)) = max([1,b2./normA])*sqrt(blktmp') ;
            idenqZ(s(1:len)) = max([sqrt(blktmp); max([normA,normC])*ones(1,len)])';
            idenqX(s(1:len)) = idenqX(s(1:len)).*(1+1e-10*randmat(len,1,0,'u'));
            idenqZ(s(1:len)) = idenqZ(s(1:len)).*(1+1e-10*randmat(len,1,0,'u'));
            X0{p} = idenqX;
            Z0{p} = idenqZ;
        elseif strcmp(pblk{1},'l');
            normC = 1+norm(C{p});
            normA = 1+sqrt(sum(At{p,1}.*At{p,1}));
            const = 10; %%--- old: const =1;
            constX = max([const,sqrt(n),sqrt(n)*b2./normA]);
            constZ = max([const,sqrt(n),normA,normC]);
            X0{p} = constX*(1+1e-10*randmat(n,1,0,'u'));
            Z0{p} = constZ*(1+1e-10*randmat(n,1,0,'u'));
        elseif strcmp(pblk{1},'u');
            X0{p} = sparse(n,1);
            Z0{p} = sparse(n,1);
        else
            error(' blk: some fields not specified correctly');
        end;
    elseif (options == 2);
        if strcmp(pblk{1},'s');
            n = sum(blktmp);
            X0{p} = scalefac*spdiags(1+1e-10*randmat(n,1,0,'u'),0,n,n);
            Z0{p} = scalefac*spdiags(1+1e-10*randmat(n,1,0,'u'),0,n,n);
        elseif strcmp(pblk{1},'q');
            s = 1+[0, cumsum(blktmp)];
            len = length(blktmp);
            idenq = zeros(sum(blktmp),1);
            idenq(s(1:len)) = 1+1e-10*randmat(len,1,0,'u');
            X0{p} = scalefac*idenq;
            Z0{p} = scalefac*idenq;
        elseif strcmp(pblk{1},'l');
            X0{p} = scalefac*(1+1e-10*randmat(n,1,0,'u'));
            Z0{p} = scalefac*(1+1e-10*randmat(n,1,0,'u'));
        elseif strcmp(pblk{1},'u');
            X0{p} = sparse(n,1);
            Z0{p} = sparse(n,1);
        else
            error(' blk: some fields not specified correctly');
        end
    end
end
%%********************************************************************


report_this_filefun(mfilename('fullpath'));

bz2 = [];
p1 = [];
p99 = [];
p50 = [];
pmab = [];
pma = [];
pmib = [];
pmi = [];
si = [];

tdiff = round((teb - t0b)*365/par1);
def = {'100','300','100','2'};
tit ='Random Zmax calculation';
prompt={'Number of bins in each time series?', 'Number of random samples drawn? (Grid-size)',...
    'Number of repeats?','Window length in years ?'};

ni2 = inputdlg(prompt,tit,1,def);
l = ni2{4};
iwl = str2double(l);
l = ni2{3};
nr = str2double(l);
l = ni2{2};
no = str2double(l);
l = ni2{1};
ni = str2double(l);

iwl0 = iwl;
iwl = floor(iwl*365/par1);
zr = (-15:0.1:15)*0;
n0 = no;
na = no;

titStr ='Warning!                                        ';

messtext= ...
    ['                                                '
    ' This rotine sometimes takes a long time!       '
    '  and may run out of memory. You can interupt   '
    ' the calculation with a ^C. The results         '
    ' calculated so far are stored in the variable   '
    ' pmab and save to a *.mat file                  '];

zmap_message_center.set_message(titStr,messtext);
figure_w_normalized_uicontrolunits(mess)

[newmatfile, newpath] = uiputfile([hodi '/*.mat'], 'Filename for saving of intermediate results ');

wai = waitbar(0,' Please Wait ...  ');
set(wai,'NumberTitle','off','Name','Percent done');;
watchon
think

con = 0;
rng('shuffle')
for k=1:nr
    na = n0; no = n0;
    while na+100 > 100;
        if na  > 100 ; na == 100; end
        con = con+1;
        l = ceil(rand([ni na])*a.Count);
        [cumu, xt] = hist(reshape(a(l,3),ni,na),(t0b:par1/365:teb));
        for j = 2:tdiff-iwl
            cu = [cumu(1:j-1,:) ; cumu(j+iwl+1:length(cumu(:,1)),:)];
            mean1 = mean(cu);
            mean2 = mean(cumu(j:j+iwl,:));
            %var1 = diag(cov(cu));
            var1 = (std(cu)).^2;;
            var2 = (std(cumu(j:j+iwl,:)).^2);
            as = (mean1 - mean2)./(sqrt(var1/(length(cumu(:,1))-iwl)+var2/iwl));
            pma = [pma max(as)];
            pmi = [pmi min(as)];
        end     % for j
        pmab = [pmab max(pma) ];
        pmib = [pmib min(pmi) ];
        pma = [];pmi =[];
        no = no - 100;
        na = no;
    end   % while na
    pmab = [pmab max(pma) ];
    pmib = [pmib min(pmi) ];
    waitbar(k/nr);
    do = ['save '  newpath newmatfile  ' pmab pma pmib pmi iwl0  con n0 nr ni '];
    eval(do,'disp(''Error while trying to save intermediate results! Permission? '') ')
end   % for k

close(wai)

figure

histogram(pmab)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
grid
xlabel('Zmax')
ylabel('Number ')
clear title
title(['ni =' num2str(ni) ', #samples = ' num2str(n0) ', #repeats=' num2str(con*nr), ', Tw= ' num2str(iwl0)]) ;

matdraw

figure

histogram(pmib)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
grid
xlabel('Zmin')
ylabel('Number ')
clear title
title(['ni =' num2str(ni) ', #samples = ' num2str(n0) ', #repeats=' num2str(con*nr), ', Tw= ' num2str(iwl0)]) ;

matdraw


function [params_obj hrf_obj] = hrf_fit(obj,TR,Runc,T,method,mode)
% HRF estimation on fmri_data class object
%
% HRF estimation function for a single voxel;
%
% Implemented methods include: IL-model (Deterministic/Stochastic), FIR
% (Regular/Smooth), and HRF (Canonical/+ temporal/+ temporal & dispersion)
%
% :Inputs:
%
%   **obj**
%        fMRI object
%
%   **TR**
%        time resolution
%
%   **Runs**
%        expermental design
%
%   **T**
%        length of estimated HRF ij seconds
%
%   **type**
%        Model type: 'FIR', 'IL', or 'CHRF'
%
%   **mode**
%        Mode
%
% :Model Types:
%
% A. **Fit HRF using IL-function**
%        Choose mode (deterministic/stochastic)
%           - 0 - deterministic aproach
%           - 1 - simulated annealing approach
%
%        Please note that when using simulated annealing approach you
%        may need to perform some tuning before use.
%
% B. **Fit HRF using FIR-model**
%        Choose mode (FIR/sFIR)
%           - 0 - FIR
%           - 1 - smooth FIR
%
% C. **Fit HRF using FIR-model**
%        Choose mode (FIR/sFIR)
%           - 0 - FIR
%           - 1 - smooth FIR
%
% :Examples:
% SIMULATE DATA AND RUN
% ::
%
%     %params for sim and fitting
%     TR = 2;   % repetition time (sec)
%     n = 200;  % time points measured (for simulation) must be multiple of 10
%     T = 30;   % duration of HRF to estimate (seconds)
%     nconds = 2; % num conditions
%     nevents = 8; % events per condition
%
%     % Create fake data
%     h = spm_hrf(TR);
%     y = zeros(n, 1);
%
%     % onsets - indicator
%     Condition = {};
%     for i = 1:nconds
%         Condition{i} = zeros(n,1);
%         wh = randperm(n);
%         Condition{i}(wh(1:nevents)) = 1;
%
%         ytmp{i} =  conv(Condition{i}, h);
%         ytmp{i} = ytmp{i}(1:n);
%     end
%
%     y = sum(cat(2, ytmp{:}), 2);
%
%     dat = fmri_data('VMPFC_mask_neurosynth.img');  % AVAILABLE ON WIKI IN MASK GALLERY
%     dat = threshold(dat, [5 Inf], 'raw-between');
%
%     v = size(dat.dat, 1); % voxels in mask
%     dat.dat = repmat(y',v, 1) + .1 * randn(v, n);
%
%     % Fit data - estimate HRFs across the brain mask
%     [params_obj hrf_obj] = hrf_fit(dat,TR, Condition, T,'FIR', 1);
%
%     hrf = fmri_data('HRF_timecourse_cond0001.img');
%     hrf = remove_empty(hrf);
%     create_figure('hrfs', 1, 2);
%     plot(hrf.dat');
%     title('Condition 1')
%     hrf = fmri_data('HRF_timecourse_cond0002.img');
%     hrf = remove_empty(hrf);
%     subplot(1, 2, 2);
%     plot(hrf.dat');
%     title('Condition 2')
%
% ..
%    Created by Martin Lindquist on 04/11/14
% ..

fprintf('HRF estimation\n')

%fhan = @(tc) hrf_fit_one_voxel(tc,TR,Runc,T,method,mode);

fhan = @(tc) hrf_fit_one_voxel_meval(tc,TR,Runc,T,method,mode);

narg_to_request = length(Runc) * 2;
myoutargs = cell(1, narg_to_request);

[myoutargs{:}] = matrix_eval_function(obj.dat',fhan);

%[hrf_vals_brain param_vals_brain] = matrix_eval_function(obj.dat',fhan);

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

numstim = length(Runc);  % number of conditions

shell_obj = mean(obj);  % shell object

for i=1:numstim

    hrf_obj_i = shell_obj;
    params_obj_i = shell_obj;

    %%%%%%%%%%%%%%%%%%%
    % Assign param data
    params_obj_i.dat = myoutargs{numstim + i};

    %%%%%%%%%%%%%%%%%%%
    % Write data
    params_obj_i.fullpath = sprintf('HRF_params_cond%04d.img', i);
    write(params_obj_i);

    %%%%%%%%%%%%%%%%%%%%
    % Assign HRF data
    hrf_obj_i.dat = myoutargs{i};

    %%%%%%%%%%%%%%%%%%%%
    % Write data
    hrf_obj_i.fullpath = sprintf('HRF_timecourse_cond%04d.img', i);
    write(hrf_obj_i);

end

hrf_obj = myoutargs(1:numstim);
params_obj = myoutargs(numstim+1 : end);

end

function varargout = hrf_fit_one_voxel_meval(tc,TR,Runc,T,method,mode)
% parse outputs into separate row vectors (for matrix_eval_function)
% outputs must be row vectors

[h, fit, e, param] = hrf_fit_one_voxel(tc,TR,Runc,T,method,mode);

varargout = {};
for i = 1:size(h, 2)
    varargout{end+1} = h(:, i)';
end

for i = 1:size(param, 2)
    varargout{end+1} = param(:, i)';
end

end


function T = convert_CPD_to_table_hidden_ps(CPD, self_val)
% CONVERT_CPD_TO_TABLE_HIDDEN_PS Convert a Gaussian CPD to a table
% function T = convert_CPD_to_table_hidden_ps(CPD, self_val)
%
% self_val must be a non-empty vector.
% All the parents are hidden.
%
% This is used by misc/convert_dbn_CPDs_to_tables

m = CPD.mean;
C = CPD.cov;
W = CPD.weights;

[ssz dpsize] = size(m);

T = zeros(dpsize, 1);
for i=1:dpsize
  T(i) = gaussian_prob(self_val, m(:,i), C(:,:,i));
end


function net = cnn_imagenet_init(varargin)
% CNN_IMAGENET_INIT  Initialize a standard CNN for ImageNet

opts.scale = 1 ;
opts.initBias = 0.1 ;
opts.weightDecay = 1 ;
%opts.weightInitMethod = 'xavierimproved' ;
opts.weightInitMethod = 'gaussian' ;
opts.model = 'alexnet' ;
opts.batchNormalization = false ;
opts = vl_argparse(opts, varargin) ;

% Define layers
switch opts.model
  case 'alexnet'
    net.normalization.imageSize = [227, 227, 3] ;
    net = alexnet(net, opts) ;
  case 'vgg-f'
    net.normalization.imageSize = [224, 224, 3] ;
    net = vgg_f(net, opts) ;
  case 'vgg-m'
    net.normalization.imageSize = [224, 224, 3] ;
    net = vgg_m(net, opts) ;
  case 'vgg-s'
    net.normalization.imageSize = [224, 224, 3] ;
    net = vgg_s(net, opts) ;
  case 'vgg-vd-16'
    net.normalization.imageSize = [224, 224, 3] ;
    net = vgg_vd(net, opts) ;
  case 'vgg-vd-19'
    net.normalization.imageSize = [224, 224, 3] ;
    net = vgg_vd(net, opts) ;
  otherwise
    error('Unknown model ''%s''', opts.model) ;
end

% final touches
switch lower(opts.weightInitMethod)
  case {'xavier', 'xavierimproved'}
    net.layers{end}.weights{1} = net.layers{end}.weights{1} / 10 ;
end
net.layers{end+1} = struct('type', 'softmaxloss', 'name', 'loss') ;

net.normalization.border = 256 - net.normalization.imageSize(1:2) ;
net.normalization.interpolation = 'bicubic' ;
net.normalization.averageImage = [] ;
net.normalization.keepAspect = true ;

% --------------------------------------------------------------------
function net = add_block(net, opts, id, h, w, in, out, stride, pad, init_bias)
% --------------------------------------------------------------------
info = vl_simplenn_display(net) ;
fc = (h == info.dataSize(1,end) && w == info.dataSize(2,end)) ;
if fc
  name = 'fc' ;
else
  name = 'conv' ;
end
net.layers{end+1} = struct('type', 'conv', 'name', sprintf('%s%s', name, id), ...
                           'weights', {{init_weight(opts, h, w, in, out, 'single'), zeros(out, 1, 'single')}}, ...
                           'stride', stride, ...
                           'pad', pad, ...
                           'learningRate', [1 2], ...
                           'weightDecay', [opts.weightDecay 0]) ;
if opts.batchNormalization
  net.layers{end+1} = struct('type', 'bnorm', 'name', sprintf('bn%d',id), ...
                             'weights', {{ones(out, 1, 'single'), zeros(out, 1, 'single')}}, ...
                             'learningRate', [2 1], ...
                             'weightDecay', [0 0]) ;
end
net.layers{end+1} = struct('type', 'relu', 'name', sprintf('relu%s',id)) ;

% -------------------------------------------------------------------------
function weights = init_weight(opts, h, w, in, out, type)
% -------------------------------------------------------------------------
% See K. He, X. Zhang, S. Ren, and J. Sun. Delving deep into
% rectifiers: Surpassing human-level performance on imagenet
% classification. CoRR, (arXiv:1502.01852v1), 2015.

switch lower(opts.weightInitMethod)
  case 'gaussian'
    sc = 0.01/opts.scale ;
    weights = randn(h, w, in, out, type)*sc;
  case 'xavier'
    sc = sqrt(3/(h*w*in)) ;
    weights = (rand(h, w, in, out, type)*2 - 1)*sc ;
  case 'xavierimproved'
    sc = sqrt(2/(h*w*out)) ;
    weights = randn(h, w, in, out, type)*sc ;
  otherwise
    error('Unknown weight initialization method''%s''', opts.weightInitMethod) ;
end

% --------------------------------------------------------------------
function net = add_norm(net, opts, id)
% --------------------------------------------------------------------
if ~opts.batchNormalization
  net.layers{end+1} = struct('type', 'normalize', ...
                             'name', sprintf('norm%s', id), ...
                             'param', [5 1 0.0001/5 0.75]) ;
end

% --------------------------------------------------------------------
function net = add_dropout(net, opts, id)
% --------------------------------------------------------------------
if ~opts.batchNormalization
  net.layers{end+1} = struct('type', 'dropout', ...
                             'name', sprintf('dropout%s', id), ...
                             'rate', 0.5) ;
end

% --------------------------------------------------------------------
function net = alexnet(net, opts)
% --------------------------------------------------------------------

net.layers = {} ;

net = add_block(net, opts, '1', 11, 11, 3, 96, 4, 0) ;
net = add_norm(net, opts, '1') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool1', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '2', 5, 5, 48, 256, 1, 2) ;
net = add_norm(net, opts, '2') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool2', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '3', 3, 3, 256, 384, 1, 1) ;
net = add_block(net, opts, '4', 3, 3, 192, 384, 1, 1) ;
net = add_block(net, opts, '5', 3, 3, 192, 256, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool5', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '6', 6, 6, 256, 4096, 1, 0) ;
net = add_dropout(net, opts, '6') ;

net = add_block(net, opts, '7', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, '7') ;

net = add_block(net, opts, '8', 1, 1, 4096, 1000, 1, 0) ;
net.layers(end) = [] ;
if opts.batchNormalization, net.layers(end) = [] ; end

% --------------------------------------------------------------------
function net = vgg_s(net, opts)
% --------------------------------------------------------------------

net.layers = {} ;
net = add_block(net, opts, '1', 7, 7, 3, 96, 2, 0) ;
net = add_norm(net, opts, '1') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool1', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 3, ...
                           'pad', [0 2 0 2]) ;

net = add_block(net, opts, '2', 5, 5, 96, 256, 1, 0) ;
net = add_norm(net, opts, '2') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool2', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ;

net = add_block(net, opts, '3', 3, 3, 256, 512, 1, 1) ;
net = add_block(net, opts, '4', 3, 3, 512, 512, 1, 1) ;
net = add_block(net, opts, '5', 3, 3, 512, 512, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool5', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 3, ...
                           'pad', [0 1 0 1]) ;

net = add_block(net, opts, '6', 6, 6, 512, 4096, 1, 0) ;
net = add_dropout(net, opts, '6') ;

net = add_block(net, opts, '7', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, '7') ;

net = add_block(net, opts, '8', 1, 1, 4096, 1000, 1, 0) ;
net.layers(end) = [] ;
if opts.batchNormalization, net.layers(end) = [] ; end

% --------------------------------------------------------------------
function net = vgg_m(net, opts)
% --------------------------------------------------------------------

net.layers = {} ;
net = add_block(net, opts, '1', 7, 7, 3, 96, 2, 0) ;
net = add_norm(net, opts, '1') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool1', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '2', 5, 5, 96, 256, 2, 1) ;
net = add_norm(net, opts, '2') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool2', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ;

net = add_block(net, opts, '3', 3, 3, 256, 512, 1, 1) ;
net = add_block(net, opts, '4', 3, 3, 512, 512, 1, 1) ;
net = add_block(net, opts, '5', 3, 3, 512, 512, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool5', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '6', 6, 6, 512, 4096, 1, 0) ;
net = add_dropout(net, opts, '6') ;

net = add_block(net, opts, '7', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, '7') ;

net = add_block(net, opts, '8', 1, 1, 4096, 1000, 1, 0) ;
net.layers(end) = [] ;
if opts.batchNormalization, net.layers(end) = [] ; end

% --------------------------------------------------------------------
function net = vgg_f(net, opts)
% --------------------------------------------------------------------

net.layers = {} ;
net = add_block(net, opts, '1', 11, 11, 3, 64, 4, 0) ;
net = add_norm(net, opts, '1') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool1', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', [0 1 0 1]) ;

net = add_block(net, opts, '2', 5, 5, 64, 256, 1, 2) ;
net = add_norm(net, opts, '2') ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool2', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '3', 3, 3, 256, 256, 1, 1) ;
net = add_block(net, opts, '4', 3, 3, 256, 256, 1, 1) ;
net = add_block(net, opts, '5', 3, 3, 256, 256, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool5', ...
                           'method', 'max', ...
                           'pool', [3 3], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '6', 6, 6, 256, 4096, 1, 0) ;
net = add_dropout(net, opts, '6') ;

net = add_block(net, opts, '7', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, '7') ;

net = add_block(net, opts, '8', 1, 1, 4096, 1000, 1, 0) ;
net.layers(end) = [] ;
if opts.batchNormalization, net.layers(end) = [] ; end

% --------------------------------------------------------------------
function net = vgg_vd(net, opts)
% --------------------------------------------------------------------

net.layers = {} ;
net = add_block(net, opts, '1_1', 3, 3, 3, 64, 1, 1) ;
net = add_block(net, opts, '1_2', 3, 3, 64, 64, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool1', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '2_1', 3, 3, 64, 128, 1, 1) ;
net = add_block(net, opts, '2_2', 3, 3, 128, 128, 1, 1) ;
net.layers{end+1} = struct('type', 'pool', 'name', 'pool2', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '3_1', 3, 3, 128, 256, 1, 1) ;
net = add_block(net, opts, '3_2', 3, 3, 256, 256, 1, 1) ;
net = add_block(net, opts, '3_3', 3, 3, 256, 256, 1, 1) ;
if strcmp(opts.model, 'vgg-vd-19')
  net = add_block(net, opts, '3_4', 3, 3, 256, 256, 1, 1) ;
end
net.layers{end+1} = struct('type', 'pool', 'name', 'pool3', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '4_1', 3, 3, 256, 512, 1, 1) ;
net = add_block(net, opts, '4_2', 3, 3, 512, 512, 1, 1) ;
net = add_block(net, opts, '4_3', 3, 3, 512, 512, 1, 1) ;
if strcmp(opts.model, 'vgg-vd-19')
  net = add_block(net, opts, '4_4', 3, 3, 512, 512, 1, 1) ;
end
net.layers{end+1} = struct('type', 'pool', 'name', 'pool4', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '5_1', 3, 3, 512, 512, 1, 1) ;
net = add_block(net, opts, '5_2', 3, 3, 512, 512, 1, 1) ;
net = add_block(net, opts, '5_3', 3, 3, 512, 512, 1, 1) ;
if strcmp(opts.model, 'vgg-vd-19')
  net = add_block(net, opts, '5_4', 3, 3, 512, 512, 1, 1) ;
end
net.layers{end+1} = struct('type', 'pool', 'name', 'pool5', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2, ...
                           'pad', 0) ;

net = add_block(net, opts, '6', 7, 7, 512, 4096, 1, 0) ;
net = add_dropout(net, opts, '6') ;

net = add_block(net, opts, '7', 1, 1, 4096, 4096, 1, 0) ;
net = add_dropout(net, opts, '7') ;

net = add_block(net, opts, '8', 1, 1, 4096, 1000, 1, 0) ;
net.layers(end) = [] ;
if opts.batchNormalization, net.layers(end) = [] ; end


function [ehmm,XW] = updateW_ehmm(ehmm,Gamma,residuals,XX,XXGXX,Tfactor,lambda)

if nargin < 6, Tfactor = 1; end
if nargin < 7, lambda = []; end

XW = zeros(size(residuals,1),size(residuals,2),ehmm.K+1);
[ehmm,XW(:,:,1:ehmm.K)] = updateW(ehmm,Gamma,residuals,XX,XXGXX,...
    Tfactor,1:ehmm.K,lambda);
XW(:,:,ehmm.K+1) = XX * ehmm.state(end).W.Mu_W;

K = size(Gamma,2); ndim = size(ehmm.train.S,1); np = size(XX,2);
for k = 1:K
    ind = (k-1)*np + (1:np);
    for n = 1:ndim
        ehmm.state_shared(n).iS_W(ind,ind) = ehmm.state(k).W.iS_W;
        ehmm.state_shared(n).S_W(ind,ind) = ehmm.state(k).W.S_W;
        ehmm.state_shared(n).Mu_W(ind) = ehmm.state(k).W.Mu_W;
    end
%     if ndim == 1
%         ehmm.state(k).W.S_W = reshape(ehmm.state(k).W.S_W,[1 np np]);
%         ehmm.state(k).W.iS_W = reshape(ehmm.state(k).W.iS_W,[1 np np]);
%     end
end

% setstateoptions;
% K = size(Gamma,2); ndim = size(ehmm.train.S,1); np = size(XX,2);
%
% Gamma = [Gamma prod(1-Gamma,2) ];
% Gamma = rdiv(Gamma,sum(Gamma,2));
%
% for k = 1:K % 1:K+1 for updating baseline as well
%
%     G = Gamma(:,k);
%     XG = bsxfun(@times, XX, G);
%     gram = XG' * XX;
%     XR = XG' * residuals;
%
%     for n = 1:ndim
%
%         if ~regressed(n), continue; end
%         ndim_n = sum(S(:,n)>0);
%
%         if ~isempty(lambda)
%             regterm = lambda * eye(np);
%             c = 1;
%         else
%             regterm = [];
%             if ~train.zeromean, regterm = ehmm.state(k).prior.Mean.iS(n); end
%             if ehmm.train.order > 0
%                 alphaterm = ...
%                     repmat( (ehmm.state(k).alpha.Gam_shape ./  ...
%                     ehmm.state(k).alpha.Gam_rate), ndim_n, 1);
%                 if ndim==1
%                     regterm = [regterm; alphaterm(:) ];
%                 else
%                     sigmaterm = repmat(ehmm.state(k).sigma.Gam_shape(S(:,n),n) ./ ...
%                         ehmm.state(k).sigma.Gam_rate(S(:,n),n), length(orders), 1);
%                     regterm = [regterm; sigmaterm .* alphaterm(:) ];
%                 end
%             end
%             c = ehmm.Omega.Gam_shape / ehmm.Omega.Gam_rate(n);
%             regterm = diag(regterm);
%         end
%
%         iS_W = regterm(Sind(:,n),Sind(:,n)) + Tfactor * c * gram(Sind(:,n),Sind(:,n));
%         iS_W = (iS_W + iS_W') / 2;
%         S_W = inv(iS_W);
%         Mu_W = Tfactor * c * S_W * XR(Sind(:,n),n);
%
%         ehmm.state(k).W.iS_W(n,:,:) = iS_W;
%         ehmm.state(k).W.S_W(n,:,:) = S_W;
%         ehmm.state(k).W.Mu_W(:,n) = Mu_W;
%
%         ind = (k-1)*np + (1:np);
%         ehmm.state_shared(n).iS_W(ind,ind) = iS_W;
%         ehmm.state_shared(n).S_W(ind,ind) = S_W;
%         ehmm.state_shared(n).Mu_W(ind) = Mu_W;
%
%     end
%
% end

end


function cut = knapsack_cheapcut(row)
% Dedicated easy cut for sum ai xi >= 0
% which appears effective on some models
if all(row(2:end)>=0) & row(1)<=0
    a = row(2:end);
    b = -row(1);
    [val,loc] = sort(a,'ascend');
    g = cumsum(val);
    m = max(find(g < -row(1)));
    ok = setdiff(loc,loc(1:m));
    b_cut = -1;
    a_cut = spalloc(1,length(val),0);
    a_cut(ok) = 1;
    cut = [b_cut a_cut];
else
    cut = [];
end


function dist = getNormGTJointDistMulti(keypointsAll,gt_annolist,jidx,bUseHeadSize,bHeadSizeFromRect,jidxGT,nrects)

if (nargin < 5)
    bHeadSizeFromRect = true;
end

if (nargin < 6)
    jidxGT = jidx;
end

scCorr = 1.33;% WAF scale correction factor when using head annopoints

dist = nan(nrects,1);
n = 0;
for imgidx = 1:length(gt_annolist)

    rect = gt_annolist(imgidx).annorect;
    for ridx = 1:length(rect)
        n = n + 1;
        if (bUseHeadSize)
            if (bHeadSizeFromRect)
                refDist = util_get_head_size(rect(ridx));
            else
                if (isfield(rect(ridx), 'annopoints'))
                    points = rect(ridx).annopoints.point;
                    p1 = util_get_annopoint_by_id(points,8);
                    p2 = util_get_annopoint_by_id(points,9);
                    refDist = scCorr*norm([p1.x p1.y] - [p2.x, p2.y]);
                else
                    assert(false);
                end
            end
        else
            refDist = util_get_torso_size(rect(ridx));
            if (isnan(refDist))
                continue;
            end
        end

        if (isfield(rect(ridx), 'annopoints'))
            points = rect(ridx).annopoints.point;
            pointsAll = [];
            for ppidx = 1:length(points)
                pointsAll = [pointsAll; points(ppidx).x points(ppidx).y];
            end
            bbox = [min(pointsAll(:,1)) min(pointsAll(:,2)) max(pointsAll(:,1)) max(pointsAll(:,2))];
            d = refDist*0.1;
            bbox = bbox + [-d -d d d];
            p = util_get_annopoint_by_id(rect(ridx).annopoints.point, jidxGT);
            if (~isempty(p))
                det = keypointsAll(imgidx).det{jidx+1};
                idxs = det(:,1) >= bbox(1) & det(:,2) >= bbox(2) & det(:,1) <= bbox(3) & det(:,2) <= bbox(4);
                detRect = det(idxs,:);
                if (~isempty(detRect))
                    [val,id] = max(detRect(:,3));
                    points_det = detRect(id,1:2);
                else
                    points_det = [inf inf];
                end
                dist(n) = norm([p.x p.y] - points_det)/refDist;
                bVis = false;
                if (bVis)
                    figure(100); clf; imagesc(imread(gt_annolist(imgidx).image.name)); axis equal; hold on;
                    vis_bbox(bbox,'g');
                    plot(points_det(:,1),points_det(:,2),'ro','MarkerSize',6,'MarkerFaceColor','r');
                    plot(p.x,p.y,'go','MarkerSize',6,'MarkerFaceColor','g');
                    text(bbox(1),bbox(2),sprintf('%1.2f',dist(n)),'FontSize',10,'color','k','BackgroundColor','g',...
                        'verticalalignment','top','horizontalalignment','left');
                end
            end
        end
    end
end


function [ objects ] = floorRectangle2hypothesis( xyzBox, rule, room_points )
%RECTANGLE2HYPOTHESIS Summary of this function goes here
%   Detailed explanation goes here
COUNTER_CLOCKWISE = true;
FITTINGRULES;
uni_room_points = zeros(8,3);
uni_room_points(POINTMAPPING{3},1) = room_points(:,1);
uni_room_points(POINTMAPPING{3},2) = room_points(:,2);
uni_room_points(POINTMAPPING{3},3) = room_points(:,3);

I = find(uni_room_points(1:4,1)<0 & uni_room_points(1:4,2)<0);
ID = rem([I I+1 I+2 I+3]-1,4) + 1;
uni_room_points = uni_room_points([ID ID+4],:);
wall_points = zeros(4,3,6);
wall_points(:,:,1) = uni_room_points([1 4 8 5],:);
wall_points(:,:,2) = uni_room_points([3 2 6 7],:);
wall_points(:,:,3) = uni_room_points([2 1 5 6],:);
wall_points(:,:,4) = uni_room_points([4 3 7 8],:);
wall_points(:,:,5) = uni_room_points([1 2 3 4],:);
wall_points(:,:,6) = uni_room_points([8 7 6 5],:);

LRDU_SURF = zeros(6,4);
LRDU_SURF(1,:) = [3 4 5 6];
LRDU_SURF(2,:) = [4 3 5 6];
LRDU_SURF(3,:) = [2 1 5 6];
LRDU_SURF(4,:) = [1 2 5 6];
LRDU_SURF(5,:) = [1 2 3 4];
LRDU_SURF(6,:) = [1 2 4 3];

normals = [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1];
normal_ids = [1 1 2 2 3 3];
main_angle = [pi/2 -pi/2 -pi 0 0];

objects = repmat(struct('out_points_w',[],'name',[],'type',[],'points',[],'x_w',[]),0,1);
object = struct('out_points_w',[],'name','other','type',[],'points',[],'x_w',zeros(7,1));
xyzBox = reshape(xyzBox', 3, 4)';

%% if left two points in lhs wall, add a hypothesis
[inside,~,~] = insideCone( wall_points(:,:,LRDU_SURF(rule,1)), xyzBox([1 4],:), 0 );
if all(inside)
    LID = LRDU_SURF(rule,1);
    wall_point = wall_points(:,:,LID);
    point5 = LineFaceIntersection(wall_point(1,:), normals(LID,:), [0 0 0], xyzBox(1,:));
    point4 = LineFaceIntersection(wall_point(1,:), normals(LID,:), [0 0 0], xyzBox(4,:));
    point6 = LineFaceIntersection(point5, normals(rule,:), [0 0 0], xyzBox(2,:));
    point1 = LineFaceIntersection(point4, normals(rule,:), [0 0 0], xyzBox(3,:));
    point3 = point4;    point2 = point1;
    point3(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);
    point2(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);

    object.out_points_w = [point1; point2; point3; point4; point5; point6];
    object.type = 13;
    object.points = object.out_points_w ./ repmat(sqrt(sum(object.out_points_w.^2,2)),1,3);
    object.x_w(1:3) = point3;
    object.x_w(4) = norm(point2-point3);
    object.x_w(5) = norm(point5-point4);
    object.x_w(6) = norm(point4-point3);
    object.x_w(7) = main_angle(rule);
    objects(end+1,1) = object;
end

%% if right two points in rhs wall, add a hypothesis
[inside,~,~] = insideCone( wall_points(:,:,LRDU_SURF(rule,2)), xyzBox([2 3],:), 0 );
if all(inside)
    RID = LRDU_SURF(rule,2);
    wall_point = wall_points(:,:,RID);
    point6 = LineFaceIntersection(wall_point(1,:), normals(RID,:), [0 0 0], xyzBox(2,:));
    point1 = LineFaceIntersection(wall_point(1,:), normals(RID,:), [0 0 0], xyzBox(3,:));
    point5 = LineFaceIntersection(point6, normals(rule,:), [0 0 0], xyzBox(1,:));
    point4 = LineFaceIntersection(point1, normals(rule,:), [0 0 0], xyzBox(4,:));
    point3 = point4;    point2 = point1;
    point3(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);
    point2(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);

    object.out_points_w = [point1; point2; point3; point4; point5; point6];
    object.type = 13;
    object.points = object.out_points_w ./ repmat(sqrt(sum(object.out_points_w.^2,2)),1,3);
    object.x_w(1:3) = point3;
    object.x_w(4) = norm(point2-point3);
    object.x_w(5) = norm(point5-point4);
    object.x_w(6) = norm(point4-point3);
    object.x_w(7) = main_angle(rule);
    objects(end+1,1) = object;
end

%% if bottom two points in bottom wall, add a hypothesis
[inside,~,~] = insideCone( wall_points(:,:,LRDU_SURF(rule,3)), xyzBox([4 3],:), 0 );
if all(inside)
    BID = LRDU_SURF(rule,3);
    wall_point = wall_points(:,:,BID);
    point4 = LineFaceIntersection(wall_point(1,:), normals(BID,:), [0 0 0], xyzBox(4,:));
    point1 = LineFaceIntersection(wall_point(1,:), normals(BID,:), [0 0 0], xyzBox(3,:));
    point5 = LineFaceIntersection(point4, normals(rule,:), [0 0 0], xyzBox(1,:));
    point6 = LineFaceIntersection(point1, normals(rule,:), [0 0 0], xyzBox(2,:));
    point3 = point4;    point2 = point1;
    point3(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);
    point2(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);

    object.out_points_w = [point1; point2; point3; point4; point5; point6];
    object.type = 13;
    object.points = object.out_points_w ./ repmat(sqrt(sum(object.out_points_w.^2,2)),1,3);
    object.x_w(1:3) = point3;
    object.x_w(4) = norm(point2-point3);
    object.x_w(5) = norm(point5-point4);
    object.x_w(6) = norm(point4-point3);
    object.x_w(7) = main_angle(rule);
    objects(end+1,1) = object;
end

%% if top two points in upper wall, add a hypothesis
[inside,~,~] = insideCone( wall_points(:,:,LRDU_SURF(rule,4)), xyzBox([1 2],:), 0 );
if all(inside)
    UID = LRDU_SURF(rule,4);
    wall_point = wall_points(:,:,UID);
    point5 = LineFaceIntersection(wall_point(1,:), normals(UID,:), [0 0 0], xyzBox(1,:));
    point6 = LineFaceIntersection(wall_point(1,:), normals(UID,:), [0 0 0], xyzBox(2,:));
    point4 = LineFaceIntersection(point5, normals(rule,:), [0 0 0], xyzBox(4,:));
    point1 = LineFaceIntersection(point6, normals(rule,:), [0 0 0], xyzBox(3,:));
    point3 = point4;    point2 = point1;
    point3(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);
    point2(normal_ids(rule)) = wall_points(1,normal_ids(rule),rule);

    object.out_points_w = [point1; point2; point3; point4; point5; point6];
    object.type = 13;
    object.points = object.out_points_w ./ repmat(sqrt(sum(object.out_points_w.^2,2)),1,3);
    object.x_w(1:3) = point3;
    object.x_w(4) = norm(point2-point3);
    object.x_w(5) = norm(point5-point4);
    object.x_w(6) = norm(point4-point3);
    object.x_w(7) = main_angle(rule);
    objects(end+1,1) = object;
end

end


function determ = dif1cyclic_determinant ( n )

%*****************************************************************************80
%
%% DIF1CYCLIC_DETERMINANT returns the determinant of the DIF1CYCLIC matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  determ = 0.0;

  return
end


function P=sympoly(varargin)
% sympoly: creator for sympoly objects
% usage: P=sympoly
% usage: P=sympoly(scalar_numeric_variable)
% usage: P=sympoly(array_numeric_variable)
% usage: P=sympoly('variablename')
% usage: sympoly varname1 varname2 varname3 ...
%
% Arguments:
%  varname - character string that represents a valid Matlab
%          variable name.
%
%          Example variable name: 'x'
%
%          or
%
%          any scalar constant or array of constants
%
% Examples of use:
%  P=sympoly       -->  creates a constant monomial, P(x) = 0
%
%  P=sympoly('x')  -->  creates a linear monomial, P(x) = x
%
%  P=sympoly(1)    -->  creates a constant monomial == 1, P(x) = 1
%
%  P=sympoly(hilb(3))  -->  creates a 3x3 matrix of constant sympoly
%                       variables, in this case a 3x3 Hilbert matrix.
%
%  sympoly a b x y -->  creates sympoly variables with those names in
%                       the caller workspace. This mode is equivalent
%                       to 4 calls:
%                        sympoly('a')
%                        sympoly('b')
%                        sympoly('x')
%                        sympoly('y')
%
%  Q=sympoly(P)    -->  copies an existing sympoly

% How was sympoly called? There are 5 options.

% sympoly('x') with an output argument --> create a linear
% sympoly variable, then return it in P.
if (nargin == 0)
  % this mode creates a scalar sympoly, as a constant == 0
  P = sympoly(0);

elseif (nargin==1)
  % Its only a single input. What kind?
  inp = varargin{1};

  if isa(inp,'sympoly')
    % Its already a sympoly. no-op.
    P = inp;
  elseif isstruct(inp) && isfield(inp,'Var') && isfield(inp,'Exponent') && isfield(inp,'Coefficient')
    % it is a struct, that wants desperately to be a sympoly
      % define this as a sympoly
      P=class(inp,'sympoly');
  elseif ischar(inp)
    % A string that contains the name of a new sympoly

    if nargout == 0
      % assign a variable with this name in the caller
      % workspace
      str = [inp,'=sympoly(''',inp,''');'];

      % evalin will put it in place in the caller workspace
      evalin('caller',str)
    else
      % Create a sympoly, to be returned as an output
      P.Var = {inp};
      P.Exponent = 1;
      P.Coefficient = 1;

      % define this as a sympoly
      P=class(P,'sympoly');

    end

  elseif isnumeric(inp) || isa(inp,'logical')
    % A numeric scalar or array

    % initialize
    P.Var = {''};
    P.Exponent = 0;
    P.Coefficient = 0;

    % replicate
    P = repmat(P,size(inp));

    % and stuff the coefficients
    for i = 1:numel(inp)
      P(i).Coefficient = inp(i);
    end

    % define this as a sympoly
    P=class(P,'sympoly');

  end

else
  % nargin is > 1. nargout must = 0
  if nargout>0
    error 'Mulitple sympolys can only be created as: "sympoly x y z w"'
  end

  % just loop over the inputs
  for i = 1:nargin
    % A list of several variables to create

    % assign variables with this name in the caller
    % workspace
    inp = varargin{i};
    str = [inp,'=sympoly(''',inp,''');'];

    % evalin will put it in place in the caller workspace
    evalin('caller',str)

  end
end

% make sure that the appropriate sympoly method is
% used whenever one of the arguments is a sympoly
% and any numeric type as the other argument.
superiorto('double','single','int8','uint8','int16', ...
  'uint16','int32','uint32','int64','uint64','logical')


function [p,f] = mrv_spm_powell(p,xi,tolsc,func,varargin)
%
% NOTE: this is a slightly modified version of spm_powell. It is
% apparently not compatible with the original.
%
% Powell optimisation method
% FORMAT [p,f] = mrv_spm_powell(p,xi,tolsc,func,varargin)
% 	p        - Starting parameter values
%	xi       - columns containing directions in which to begin
% 	           searching.
% 	tolsc    - stopping criteria
% 	           - optimisation stops when
% 	             sqrt(sum(((p-p_prev)./tolsc).^2))<1
% 	func     - name of evaluated function
% 	varargin - remaining arguments to func (after p)
%
% 	p        - final parameter estimates
% 	f        - function value at minimum
%
%_______________________________________________________________________
% Method is based on Powell's optimisation method from Numerical Recipes
% in C (Press, Flannery, Teukolsky & Vetterling).
%_______________________________________________________________________
% @(#)spm_powell.m	2.3 John Ashburner 01/09/28

p     = p(:);
f     = feval(func,p,varargin{:});
for iter=1:128,
	fprintf('iteration %d...\n', iter);
	pp   = p;
	fp   = f;
	del  = 0;
	for i=1:length(p),
		ft = f;
		[p,junk,f] = linmin(p,xi(:,i),func,f,tolsc,varargin{:});
		if abs(ft-f) > del,
			del  = abs(ft-f);
			ibig = i;
		end;
	end;
	if sqrt(sum(((p(:)-pp(:))./tolsc(:)).^2))<1, return; end;
	ft = feval(func,2.0*p-pp,varargin{:});
	if ft < f,
		[p,xi(:,ibig),f] = linmin(p,p-pp,func,f,tolsc,varargin{:});
	end;
end;
warning('Too many optimisation iterations');
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [p,pi,f] = linmin(p,pi,func,f,tolsc,varargin)
% Line search for minimum.

global lnm % used in linmineval
lnm      = struct('p',p,'pi',pi,'func',func,'args',[]);
lnm.args = varargin;

linmin_plot('Init', 'Line Minimisation','Function','Parameter Value');
linmin_plot('Set', 0, f);

tol      = 1/sqrt(sum((pi(:)./tolsc(:)).^2));
t        = bracket(f);
[f,pmin] = brents(t,tol);
pi       = pi*pmin;
p        = p + pi;

for i=1:length(p), fprintf('%-8.4g ', p(i)); end;
fprintf('| %.5g\n', f);
linmin_plot('Clear');

return;
%_______________________________________________________________________

%_______________________________________________________________________
function f = linmineval(p)
% Reconstruct parameters and evaluate.

global lnm % defined in linmin
pt = lnm.p+p.*lnm.pi;
f  = feval(lnm.func,pt,lnm.args{:});
linmin_plot('Set',p,f);
return;
%_______________________________________________________________________

%_______________________________________________________________________
function t = bracket(f)
% Bracket the minimum (t(2)) between t(1) and t(2)

gold   = (1+sqrt(5))/2; % Golden ratio

t(1)   = struct('p',0,'f',f);
t(2).p = 1;
t(2).f = linmineval(t(2).p);

% if not better then swap
if t(2).f > t(1).f,
	tmp  = t(1);
	t(1) = t(2);
	t(2) = tmp;
end;

t(3).p = t(2).p + gold*(t(2).p-t(1).p);
t(3).f = linmineval(t(3).p);

while t(2).f > t(3).f,

	% fit a polynomial to t
	tmp = cat(1,t.p)-t(2).p;
	pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

	% minimum is when gradient of polynomial is zero
	% sign of pol(3) (the 2nd deriv) should be +ve
	d   = -pol(2)/(2*pol(3)+eps);
	u.p = t(2).p+d;

	ulim = t(2).p+100*(t(3).p-t(2).p);
	if (t(2).p-u.p)*(u.p-t(3).p) > 0.0,
		% u is between t(2) and t(3)
		u.f = linmineval(u.p);
		if u.f < t(3).f,
			% minimum between t(2) and t(3) - done
			t(1) = t(2);
			t(2) = u;
			return;
		elseif u.f > t(2).f,
			% minimum between t(1) and u - done
			t(3) = u;
			return;
		end;
		% try golden search instead
		u.p = t(3).p+gold*(t(3).p-t(2).p);
		u.f = linmineval(u.p);

	elseif (t(3).p-u.p)*(u.p-ulim) > 0.0
		% u is between t(3) and ulim
		u.f = linmineval(u.p);
		if u.f < t(3).f,
			% still no minimum as function is still decreasing
			% t(1) = t(2);
			t(2) = t(3);
			t(3) = u;
			u.p  = t(3).p+gold*(t(3).p-t(2).p);
			u.f  = linmineval(u.p);
		end;

	elseif (u.p-ulim)*(ulim-t(3).p) >= 0.0,
		% gone too far - constrain it
		u.p = ulim;
		u.f = linmineval(u.p);

	else,
		% try golden search instead
		u.p = t(3).p+gold*(t(3).p-t(2).p);
		u.f = linmineval(u.p);
	end;

	% Move all 3 points along
	t(1) = t(2);
	t(2) = t(3);
	t(3) = u;
end;
return;
%_______________________________________________________________________

%_______________________________________________________________________
function [f,p] = brents(t, tol)
% Brent's method for line searching - given that minimum is bracketed

% 1 - golden ratio
Cgold = 1 - (sqrt(5)-1)/2;

% Current and previous displacements
d     = Inf;
pd    = Inf;

% t(1) and t(3) bracket the minimum
if t(1).p>t(3).p,
	brk(1) = t(3).p;
	brk(2) = t(1).p;
else,
	brk(1) = t(1).p;
	brk(2) = t(3).p;
end;

% sort t into best first order
tmp  = t(1);
t(1) = t(2);
t(2) = tmp;
if t(2).f>t(3).f,
	tmp  = t(2);
	t(2) = t(3);
	t(3) = tmp;
end;

for iter=1:128,
	% check stopping criterion
	if abs(t(1).p - 0.5*(brk(1)+brk(2)))+0.5*(brk(2)-brk(1)) <= 2*tol,
		p = t(1).p;
		f = t(1).f;
		return;
	end;

	% keep last two displacents
	ppd = pd;
	pd  = d;

	% fit a polynomial to t
	tmp = cat(1,t.p)-t(1).p;
	pol = pinv([ones(3,1) tmp tmp.^2])*cat(1,t.f);

	% minimum is when gradient of polynomial is zero
	d   = -pol(2)/(2*pol(3)+eps);
	u.p = t(1).p+d;

	% check so that displacement is less than the last but two,
	% that the displaced point is between the brackets
	% and (not sure if it is necessary) that the solution is a minimum
	% rather than a maximum
	eps2 = 2*eps*abs(t(1).p)+eps;
	if abs(d) >= abs(ppd)/2 | u.p <= brk(1)+eps2 | u.p >= brk(2)-eps2 | pol(3)<=0,
		% if criteria are not met, then golden search into the larger part
		if t(1).p >= 0.5*(brk(1)+brk(2)),
			d = Cgold*(brk(1)-t(1).p);
		else,
			d = Cgold*(brk(2)-t(1).p);
		end;
		u.p = t(1).p+d;
	end;

	% FUNCTION EVALUATION
	u.f = linmineval(u.p);

	% Insert the new point into the appropriate position and update
	% the brackets if necessary
	if u.f <= t(1).f,
		if u.p >= t(1).p, brk(1)=t(1).p; else, brk(2)=t(1).p; end;
		t(3) = t(2);
		t(2) = t(1);
		t(1) = u;
	else,
		if u.p < t(1).p, brk(1)=u.p; else, brk(2)=u.p; end;
		if u.f <= t(2).f | t(1).p==t(2).p,
			t(3) = t(2);
			t(2) = u;
		elseif u.f <= t(3).f | t(1).p==t(3).p | t(2).p==t(3).p,
			t(3) = u;
		end;
	end;
end;
warning('Too many iterations in Brents');
return;
%_______________________________________________________________________

%_______________________________________________________________________
function linmin_plot(action,arg1,arg2,arg3,arg4)
% Visual output for line minimisation
global linminplot
%-----------------------------------------------------------------------
if (nargin == 0)
	linmin_plot('Init');
else
	% initialize
	%---------------------------------------------------------------
	if (strcmp(lower(action),'init'))
		if (nargin<4)
			arg3 = 'Function';
			if (nargin<3)
				arg2 = 'Value';
				if (nargin<2)
					arg1 = 'Line minimisation';
				end
			end
		end
		fg = spm_figure('FindWin','Interactive');

		if ~isempty(fg)
			linminplot = struct('pointer',get(fg,'Pointer'),'name',get(fg,'Name'),'ax',[]);
			linmin_plot('Clear');
			set(fg,'Pointer','watch');
			% set(fg,'Name',arg1);
			linminplot.ax = axes('Position', [0.15 0.1 0.8 0.75],...
				'Box', 'on','Parent',fg);
			lab = get(linminplot.ax,'Xlabel');
			set(lab,'string',arg3,'FontSize',10);
			lab = get(linminplot.ax,'Ylabel');
			set(lab,'string',arg2,'FontSize',10);
			lab = get(linminplot.ax,'Title');
			set(lab,'string',arg1);
			line('Xdata',[], 'Ydata',[],...
				'LineWidth',2,'Tag','LinMinPlot','Parent',linminplot.ax,...
				'LineStyle','-','Marker','o');
			drawnow;
		end

	% reset
	%---------------------------------------------------------------
	elseif (strcmp(lower(action),'set'))
		F = spm_figure('FindWin','Interactive');
		br = findobj(F,'Tag','LinMinPlot');
		if (~isempty(br))
			[xd,indx] = sort([get(br,'Xdata') arg1]);
			yd = [get(br,'Ydata') arg2];
			yd = yd(indx);
			set(br,'Ydata',yd,'Xdata',xd);
			drawnow;
		end

	% clear
	%---------------------------------------------------------------
	elseif (strcmp(lower(action),'clear'))
		fg = spm_figure('FindWin','Interactive');
		if isstruct(linminplot),
			if ishandle(linminplot.ax), delete(linminplot.ax); end;
			set(fg,'Pointer',linminplot.pointer);
			set(fg,'Name',linminplot.name);
		end;
		spm_figure('Clear',fg);
		drawnow;
	end;
end
%_______________________________________________________________________


function Ascale = cmultiscale(scale, A)
% Multiplies the 2D slices in a 3D array by individual scalars.
%
% function Ascale = cmultiscale(scale, A)
%
% Basically the same as multiscale but is compatible with structs with
% fields real and imag, which means this can be used for automatic
% differentiation with complex arrays in older Matlab versions.
%
% See also: manoptADhelp multiscale

% This file is part of Manopt: www.manopt.org.
% Original author: Xiaowen Jiang, Aug. 31, 2021.
% Contributors: Nicolas Boumal
% Change log:

    if isnumeric(A) && isnumeric(scale)

        Ascale = multiscale(scale, A);

    else

        A = tocstruct(A);
        scale = tocstruct(scale);

        assert(ndims(A.real) == 3, ...
           'cmultiscale is only well defined for 3D arrays.');

        [~, ~, N] = size(A.real);

        assert(numel(scale.real) == N, ...
           ['scale must be a vector whose length equals the third ' ...
            'dimension of A, that is, the number of 2D matrix slices ' ...
            'in the 3D array A. It can also be a struct with fields ' ...
            'real and imag with size as stated above.']);

        scale.real = reshape(scale.real, 1, 1, N);
        scale.imag = reshape(scale.imag, 1, 1, N);
        Ascale = cdottimes(A, scale);

    end

end

% Test code
% n = 3; m = 5; N = 17;
% A = randn(n, m, N);
% scale = randn(N, 1);
% Z = multiscale(scale, A) - cmultiscale(scale, A);
% norm(Z(:))
% A = randn(n, m, N) + 1i*randn(n, m, N);
% Z = multiscale(scale, A) - cmultiscale(scale, A);
% norm(Z(:))
% scale = randn(N, 1) + 1i*randn(N, 1);
% Z = multiscale(scale, A) - cmultiscale(scale, A);
% norm(Z(:))
% B.real = real(A); B.imag = imag(A);
% Z = cmultiscale(scale, B);
% Zr = real(multiscale(scale, A)) - Z.real;
% Zi = imag(multiscale(scale, A)) - Z.imag;
% norm(Zr(:))
% norm(Zi(:))
% scalebis = tocstruct(scale);
% Z = cmultiscale(scalebis, B);
% Zr = real(multiscale(scale, A)) - Z.real;
% Zi = imag(multiscale(scale, A)) - Z.imag;
% norm(Zr(:))
% norm(Zi(:))


function [element2edges, edge2nodes]=getEdges(elements)
%function: [element2edges, edge2nodes]=edge_numbering(elements)
%requires: deleterepeatedrows
%generates edges of (triangular) triangulation defined in elements
%elements is matrix, whose rows contain numbers of its element nodes
%element2edges returns edges numbers of each triangular element
%edge2nodes returns two node numbers of each edge
%example in 2D: [element2edges, edge2nodes]=getEdges([1 2 3; 2 4 3])
%example in 3D: [element2edges, edge2nodes]=getEdges([1 2 3 4; 1 2 3 5; 1 2 4 6])

%2D case
if (size(elements,2)==3)
    %extracts sets of edges
    edges1=elements(:,[2 3]);
    edges2=elements(:,[3 1]);
    edges3=elements(:,[1 2]);

    %as sets of their nodes (vertices)
    vertices=zeros(size(elements,1)*3,2);
    vertices(1:3:end,:)=edges1;
    vertices(2:3:end,:)=edges2;
    vertices(3:3:end,:)=edges3;

    %repeated sets of nodes (joint edges) are eliminated
    [edge2nodes,element2edges]=deleterepeatedrows(vertices);
    element2edges=reshape(element2edges,3,size(elements,1))';
end

%3D case
if (size(elements,2)==4)
    %extracts sets of edges
    edges1=elements(:,[2 3]);
    edges2=elements(:,[3 1]);
    edges3=elements(:,[1 2]);
    edges4=elements(:,[3 4]);
    edges5=elements(:,[1 4]);
    edges6=elements(:,[2 4]);

    %as sets of their nodes (vertices)
    vertices=zeros(size(elements,1)*6,2);
    vertices(1:6:end,:)=edges1;
    vertices(2:6:end,:)=edges2;
    vertices(3:6:end,:)=edges3;
    vertices(4:6:end,:)=edges4;
    vertices(5:6:end,:)=edges5;
    vertices(6:6:end,:)=edges6;

    %repeated sets of nodes (joint edges) are eliminated
    [edge2nodes,element2edges]=deleterepeatedrows(vertices);
    element2edges=reshape(element2edges,6,size(elements,1))';
end


function pwl_interp_2d_scattered_test ( )

%*****************************************************************************80
%
%% PWL_INTERP_2D_SCATTERED_TEST tests the PWL_INTERP_2D_SCATTERED library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2012
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../pwl_interp_2d_scattered' )
  addpath ( '../test_interp_2d' )
  addpath ( '../r8lib' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PWL_INTERP_2D_SCATTERED_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test PWL_INTERP_2D_SCATTERED.\n' );
  fprintf ( 1, '  The R8LIB library is needed.\n' );
  fprintf ( 1, '  This test also needs the TEST_INTERP_2D library.\n' );

  pwl_interp_2d_scattered_test01 ( );
  pwl_interp_2d_scattered_test02 ( );
%
%  Numerical tests.
%
  prob_num = f00_num ( );

  for prob = 1 : prob_num
    pwl_interp_2d_scattered_test03 ( prob );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PWL_INTERP_2D_SCATTERED_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../pwl_interp_2d_scattered' )
  rmpath ( '../test_interp_2d' )
  rmpath ( '../r8lib' )

  return
end


function [Statistics,significantFeatures] = performStatisticalAnalysis(sampleData,sampleInformation,varargin)
% This function determines if there is a significant difference between
% features computed for two or more groups in a cohort of samples. If the
% cohort contains two groups, the Wilcoxon rank sum test is used. If the
% cohort contains three or more groups, the Kruskal Wallis test is used.
%
% USAGE
% [Statistics,significantFeatures] = performStatisticalAnalysis(sampleData,sampleInformation,stratification)
%
% INPUTS
% sampleData           Table with input data to analyze (e.g., fluxes) with
%                      computed features as rows and sample IDs as columns
% sampleInformation    Table with information on analyzed samples including
%                      group classification with sample IDs as rows
%
% OPTIONAL INPUT
% stratification       Column header containing the desired group
%                      classification in sampleInformation table. If not
%                      provided, the second column will be used.
% groupTest            Decides whether Kruskal-Wallis test(default) or
%                      ANOVA should be used for group comparisons.
%                      Allowed inputs: "Kruskal-Wallis","ANOVA"
%
% OUTPUTS
% Statistics           Table with results of statistical tests for each
%                      computed feature
% significantFeatures  Table with input data reduced to only features that
%                      were statistically significant
%
% AUTHOR
%       - Almut Heinken, 12/2020

parser = inputParser();
parser.addRequired('sampleData', @iscell);
parser.addRequired('sampleInformation', @iscell);
parser.addParameter('stratification', '', @ischar);
parser.addParameter('groupTest', 'Kruskal-Wallis', @ischar);

parser.parse(sampleData, sampleInformation, varargin{:});

sampleData = parser.Results.sampleData;
sampleInformation = parser.Results.sampleInformation;
stratification = parser.Results.stratification;
groupTest = parser.Results.groupTest;

if ~any(strcmp(groupTest,{'Kruskal-Wallis','ANOVA'}))
    error('Wrong input for group test!')
end

% find the column with the sample information to analyze the samples by
if ~isempty(stratification)
stratCol=find(strcmp(sampleInformation(1,:),stratification));
else
    stratCol=2;
end

% delete empty columns in the data
sampleData{1,1}='Averages';
delIndices =cellfun(@isempty, sampleData(1,:));
sampleData(:,delIndices)=[];
% delete columns that are all zeros
cnt=1;
delArray=[];
for i=2:size(sampleData,2)
    if abs(sum(str2double(sampleData(2:end,i))))<0.0000001
        delArray(1,cnt)=i;
        cnt=cnt+1;
    end
end
sampleData(:,delArray)=[];

% delete metadata entries not in the sample data
[C,IA]=setdiff(sampleInformation(:,1),sampleData(1,:),'stable');
sampleInformation(IA(2:end),:)=[];

groups=unique(sampleInformation(2:end,stratCol));

if length(groups) > 1

% Fill out table header
if length(groups)==2
    Statistics={'Feature','Description','p_value_before_FDR_Corr','p_value_after_FDR_Corr','Decision','Rank sum statistic','Z-statistics'};
elseif length(groups)>2
    if strcmp(groupTest,'Kruskal-Wallis')
        Statistics={'Feature','Description','p_value_before_FDR_Corr','p_value_after_FDR_Corr','Decision','Degrees of freedom','Chi-sq'};
    elseif strcmp(groupTest,'ANOVA')
        Statistics={'Feature','Description','p_value_before_FDR_Corr','p_value_after_FDR_Corr','Decision','Degrees of freedom','Sum of squares'};
    end
end
cnt=size(Statistics,2)+1;
for i=1:length(groups)
    Statistics{1,cnt}=['Average_',groups{i}];
    cnt=cnt+1;
    Statistics{1,cnt}=['StandardDeviation_',groups{i}];
    cnt=cnt+1;
end

% test if sample information and sample names match
C=intersect(sampleInformation(:,1),sampleData(1,:));
if isempty(C)
    error('Sample IDs are not present as column headers of the sample data table. Consider transposing sample data table.')
end

for j=2:size(sampleData,2)
    if ~isempty(find(strcmp(sampleInformation(:,1),sampleData{1,j})))
        group{j-1,1}=sampleInformation{find(strcmp(sampleInformation(:,1),sampleData{1,j})),stratCol};
    end
end

%% calculate the statistics
for i=2:size(sampleData,1)
    Statistics{i,1}=sampleData{i,1};
    if contains(version,'(R202') % for Matlab R2020a and newer
        dataAll=cell2mat(sampleData(i,2:end));
    else
        dataAll=str2double(sampleData(i,2:end));
    end

    % separate data by group
    for j=1:length(groups)
        dataGrouped{j}=dataAll';
        delInd=find(~strcmp(group,groups{j}));
        dataGrouped{j}(delInd,:)=[];
    end

    if length(groups)==2
        % use Wilcoxon rank sum test

        [p,h,stats] = ranksum(dataGrouped{1},dataGrouped{2},'method','approximate');
        if isnan(p)
            p=1;
        end
        Statistics{i,3}=p;
        Statistics{i,5}=h;
        Statistics{i,6}=stats.ranksum;
        Statistics{i,7}=stats.zval;

    elseif length(groups)>2
        if strcmp(groupTest,'Kruskal-Wallis')
            % use Kruskal Wallis test
            [p,ANOVATAB] = kruskalwallis(dataAll,group,'off');
            Statistics{i,3}=p;
            if ANOVATAB{2,6} ==0
                Statistics{i,5}='0';
            else
                Statistics{i,5}='1';
            end
            Statistics{i,6}=ANOVATAB{2,3};
            Statistics{i,7}=ANOVATAB{2,5};
        elseif strcmp(groupTest,'ANOVA')
            [p,tbl,stats] = anova1(dataAll,group,'off');
            Statistics{i,3}=p;
            if p<0.05
                Statistics{i,5}='1';
            else
                Statistics{i,5}='0';
            end
            Statistics{i,6}=tbl{2,3};
            Statistics{i,7}=tbl{2,2};
        end
    end

    for j=1:length(groups)
        findCol=find(strcmp(Statistics(1,:),['Average_',groups{j}]));
        Statistics{i,findCol}=mean(dataGrouped{j});
        findCol=find(strcmp(Statistics(1,:),['StandardDeviation_',groups{j}]));
        Statistics{i,findCol}=std(dataGrouped{j});
    end
end

%% correct for false discovery (FDR) rate
pAverages=cell2mat(Statistics(2:end,3));
fdr = mafdr(pAverages,'BHFDR', true);
Statistics(2:end,4)=num2cell(fdr);

for i=2:size(Statistics,1)
    if Statistics{i,4} <0.05
        Statistics{i,5} = 1;
    else
        Statistics{i,5} = 0;
    end
end

%% save only the significant entries as a spreadsheet
nsMets={};
cnt=1;
for i=2:size(Statistics,1)
    if Statistics{i,5}==0
        nsMets{cnt,1}=Statistics{i,1};
        cnt=cnt+1;
    end
end
[C,ia,ib] = intersect(sampleData(:,1),nsMets);
significantFeatures=sampleData;
significantFeatures(ia,:)=[];

%% add reaction/metabolite annotations if possible
database=loadVMHDatabase;

for i=2:size(Statistics,1)
    feat=Statistics{i,1};
    if ~any(~contains(Statistics(:,1),'[fe]'))
        feat=strrep(feat,'EX_','');
        feat=strrep(feat,'[fe]','');
    end
    if ~isempty(find(strcmp(database.metabolites(:,1),feat)))
        Statistics{i,2}=database.metabolites{find(strcmp(database.metabolites(:,1),feat)),2};
    elseif ~isempty(find(strcmp(database.reactions(:,1),feat)))
        Statistics{i,2}=database.reactions{find(strcmp(database.reactions(:,1),feat)),2};
    else
        Statistics{i,2}='NA';
    end
end

else
    Statistics = {};
    significantFeatures = {};
end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Calibrating kl, ku, mu and nu for a sabr model
% the prices are calculated from known sabr parameters

function y = of_sabr(a, b, r, n, f, k, t, mu, nu, kl, ku, call, put)
% objective function for SABR calibration
    [kl, cl, bl, al, ku, cu, bu, au] = ...
        psabr_param_2(a, b, r, n, f, k, t,mu,nu, kl, ku);

    callkl = interp1(k,call,kl);        % get value at kl
    putku = interp1(k,put,ku);          % get value at ku

    % using only call or put prices is enough
    % can use put left of atm and calls right!!!
    % that is put for [kl,f) and calls (f,ku]
    y = (sprice_5(a, b, r, n, f, kl, t, kl, ku, ...
        mu, cl, bl,al, nu, cu,bu,au, 1) ...
        - callkl)^2;           % call prics
    y = y + (sprice_5(a, b, r, n, f, 0, t, kl, ku, ...
        mu, cl, bl,al, nu, cu,bu,au, 0) ...
        - f)^2;                % atm price
    y = y + (sprice_5(a, b, r, n, f, ku, t, kl, ku, ...
        mu, cl, bl,al, nu, cu,bu,au, 0) ...
        - putku)^2;            % put prices


function [ detection, Q, G ] = detector( face_image, model )
%DETECTOR Given normalized image frame face_image and model structure this
%function detects positions of significant points on human face and returns
%its positions.
%
% INPUT:
%   face_image      ...     normalized image frame (see function getNormalizedFrame)
%   model           ...     structure holding parameters of model (see
%                           exampleUsageDetector.m for details how to create it)
%
% OUTPUT:
%   detection       ...     matrix 2xM format [x_0 ... x_{M-1};
%                                              y_0 ... y_{M-1}]
%                           detected positions of significant points
%
% 18-08-10 Michal Uricar
% 08-08-11 Michal Uricar, argmax_mex modification

    model.data.Images = face_image(:);

%     Psi = model.fgetPsiMat(model.data, 1);
%     Psi = model.fgetPsiMat(model.data);

%     [ q, g ] = model.fgetQG(model.data.options, model.W, Psi, model.data.mapTable);
%     detection = model.fargmax(model.data.options, q, g);

    [Psi, PsiSparse] = model.fgetPsiMat(model.data, 1);

    if (nargout > 1)
        [ q, g ] = model.fgetQG(model.data.options, model.W, Psi, model.data.mapTable);
        [detection, Q, G] = model.fargmax(model.data.options, q, g);
    else
        %detection = model.fargmax(model.data.options, q, g);
        detection = model.argmax_mex(model.data.options, model.W, model.data.mapTable, PsiSparse);
    end

end


function plot_3d_odomap(groundTruthFile, odoMapFile, ...
    xOdoMapScaling, yOdoMapScaling, zOdoMapScaling, ...
    xOdoMapTrans, yOdoMapTrans, zOdoMapTrans, ...
    xGtScaling, yGtScaling, zGtScaling)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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/>.

    % load ground truth data
    [frameId, gt_x, gt_y, gt_z, gt_rx, gt_ry, gt_rz] = load_ground_truth_data(groundTruthFile);

    % load experience map data
    [id, odomap_x, odomap_y, odomap_z, odomap_yaw] = load_exp_map_data(odoMapFile);

    figure('color',[1 1 1]);
    hold on

    plot3(odomap_y * yOdoMapScaling + yOdoMapTrans, ...
        odomap_x * xOdoMapScaling + xOdoMapTrans, ...
        odomap_z * zOdoMapScaling + zOdoMapTrans, '.b', 'MarkerSize',7);
    plot3((gt_x - gt_x(1)) * xGtScaling,(gt_y - gt_y(1)) * yGtScaling, (gt_z - gt_z(1)) * zGtScaling, '.r','MarkerSize',7);
    hold off;

    view(3)
% view(0, 90);
    grid on
%     xlabel('x');
%     ylabel('y');
    xl = xlabel('x','FontSize',18);
    yl = ylabel('y','FontSize',18);
    zlabel('z','FontSize',18);
    set(xl,'Rotation',15);
    set(yl,'Rotation',-30);
%     title('3D Experience Map');
    %                     legend('Result','Truth' ,'1');
    % SynPanData
    axis([-2 16 -20 5 -1 3]);

% SynPerData
%     axis([-5 20 -20 5 -1 3]);
    %                     axis equal
%     legend('Experience map','Ground truth', 'location', '0');
    hh = legend('OM','GT');

    set(hh,'position',[.76 .82 .05 .05]);
    set(gca,'FontSize',18); % axis font
    set (gcf,'Position',[500,300,400,300], 'color','w')
    axis on
    rotate3d on
end


%%*************************************************************************
%% parameters.m: set OPTIONS structure to specify default
%%               parameters for sqlp.m
%%
%% OPTIONS.vers        : version of direction to use.
%%                       1 for HKM direction
%%                       2 for NT direction
%%                       0 for the default (which uses the HKM direction if
%%                       semidefinite blocks exist; and NT direction of SOCP problms)
%% OPTIONS.gam         : step-length parameter,
%% OPTIONS.predcorr    : whether to use Mehrotra predictor-corrector.
%% OPTIONS.expon       : exponent in decrease of centering parameter sigma.
%% OPTIONS.gaptol      : tolerance for duality gap as a fraction of the
%%                       value of the objective functions.
%% OPTIONS.inftol      : tolerance for stopping due to suspicion of
%%                       infeasibility.
%% OPTIONS.steptol     : toloerance for stopping due to small steps.
%% OPTIONS.maxit       : maximum number of iteration allowed
%% OPTIONS.printlevel  : 3, if want to display result in each iteration,
%%                       2, if want to display only summary,
%%                       1, if want to display warning message,
%%                       0, no display at all.
%% OPTIONS.stoplevel   : 2, if want to automatically detect termination;
%%                       1, if want to automatically detect termination, but
%%                          restart automatically with a new iterate
%%                          when the algorithm stagnants because of tiny step-lengths.
%%                       0, if want the algorithm to continue forever except for
%%                          successful completion, maximum number of iterations, or
%%                          numerical failures. Note, do not change this field unless
%%                          you very sure.
%% OPTIONS.scale_data  : 1, if want to scale the data before solving the problem,
%%                          else = 0
%% OPTIONS.rmdepconstr : 1, if want to remove nearly dependent constraints,
%%                          else = 0.
%% OPTIONS.smallblkdim : block-size threshold determining what method to compute the
%%                       schur complement matrix corresponding to semidefintie block.
%%                       NOTE: this number should be small, say less than 20.
%% OPTIONS.parbarrier  : parameter values of the log-barrier terms in the SQLP problem.
%%                       Default = [],  meaning that the parameter values are all 0.
%% OPTIONS.schurfun    : [], if no user supplied routine to compute the Schur matrix,
%%                       else, it is a cell array where each cell is either [],
%%                       or contains a string that is the file name where the Schur matrix
%%                       of the associated block data is computed.
%%                       For example, if the SQLP data has the block structure
%%                       blk{1,1} = '1'; blk{1,2} = 10;
%%                       blk{2,1} = 's'; blk{2,2} = 50;
%%                       and
%%                       OPTIONS.schurfun{1} = [];
%%                       OPTIONS.schurfun{2} = 'myownschur', where
%%                             'myownschur' is a function with the calling sequence:
%%                              function   schur = myownschur(X2,Z2inv,schurfun_par(2,:));
%%                       This means that for the first block, the Schur
%%                       matrix is computed by the default method in SDPT3,
%%                       and for the second block, the user supplies the
%%                       routine to compute the associated Schur matrix.
%% OPTIONS.schurfun_par: [], if no user supplied routine to compute the Schur matrix,
%%                       else, it is a cell array where the p-th row is either [],
%%                       or is a cell array containing the parameters needed in
%%                       the user supplied Schur routine OPTIONS.schurfun{p}.
%%                       For example, for the block structure described
%%                       above, we may have:
%%                       OPTIONS.schurfun_par{1} = [];
%%                       OPTIONS.schurfun_par{2,1} = par1;
%%                       OPTIONS.schurfun_par{2,2} = par2;
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

function OPTIONS = sqlparameters

OPTIONS.vers           = 0;
OPTIONS.gam            = 0;
OPTIONS.predcorr       = 1;
OPTIONS.expon          = 1;
OPTIONS.gaptol         = 1e-8;
OPTIONS.inftol         = 1e-8;
OPTIONS.steptol        = 1e-6;
OPTIONS.maxit          = 100;
OPTIONS.printlevel     = 3;
OPTIONS.stoplevel      = 1; %% do not change this field unless you very sure.
OPTIONS.scale_data     = 0;
OPTIONS.spdensity      = 0.4;
OPTIONS.rmdepconstr    = 0;
OPTIONS.smallblkdim    = 50;
OPTIONS.parbarrier     = [];
OPTIONS.schurfun       = [];
OPTIONS.schurfun_par   = [];
%%*************************************************************************


function test_bug2186

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

data = [];
for i=1:10
  data.label{i} = num2str(i);
end
for i=1:20
  data.time{i} = (1:1000)/1000;
  data.trial{i} = randn(10, 1000);
end

cfg = [];
cfg.trials = 1:10;
data1 = ft_selectdata(cfg, data);
assert(data1.trial{1}(1)==data.trial{1}(1));

cfg = [];
cfg.trials = 11:20;
data2 = ft_selectdata(cfg, data);
assert(data2.trial{1}(1)==data.trial{11}(1));

%%

cfg = [];
cfg.keeptrials = 'no';
cfg.covariance = 'yes';
timelock1 = ft_timelockanalysis(cfg, data1);
timelock2 = ft_timelockanalysis(cfg, data2);

append12 = ft_appendtimelock([], timelock1, timelock2);
assert(isfield(append12, 'trial')); % renamed from avg
assert(isfield(append12, 'dof'));
assert(isfield(append12, 'var'));
assert(isfield(append12, 'cov')); % this one is new

%%

timelock1a = timelock1;
for i=1:10
  timelock1a.label{i} = num2str(i+10);
end
append1a = ft_appendtimelock([], timelock1, timelock1a);
assert(isequal(size(append1a.cov), [20 20]));
assert(isnan(append1a.cov(20, 1))); % it should be a block diagonal matrix with nan off-diagonal

%%

cfg = [];
cfg.keeptrials = 'yes';
cfg.covariance = 'yes';
timelock1 = ft_timelockanalysis(cfg, data1);
timelock2 = ft_timelockanalysis(cfg, data2);

append12 = ft_appendtimelock([], timelock1, timelock2);
assert(isfield(append12, 'trial')); % avg, dof and var are not present
assert(isfield(append12, 'cov')); % this one is new

%%

timelock1a = timelock1;
for i=1:10
  timelock1a.label{i} = num2str(i+10);
end
append1a = ft_appendtimelock([], timelock1, timelock1a);
assert(isequal(size(append1a.cov), [10 20 20]));
assert(isnan(append1a.cov(1, 20, 1))); % it should be a block diagonal matrix with nan off-diagonal


function [mu, varsigma] = fgplvmPosteriorMeanVar(model, X);

% FGPLVMPOSTERIORMEANVAR Mean and variances of the posterior at points given by X.
% FORMAT
% DESC returns the posterior mean and variance for a given set of
% points.
% ARG model : the model for which the posterior will be computed.
% ARG x : the input positions for which the posterior will be
% computed.
% RETURN mu : the mean of the posterior distribution.
% RETURN sigma : the variances of the posterior distributions.
%
% SEEALSO : gpPosteriorMeanVar, fgplvmCreate
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% FGPLVM

[mu, varsigma] = gpPosteriorMeanVar(model, X);


%% Grain Boundary Tutorial
% A quick guide to grain boundary analysis
%
%% Grain boundaries generation
%
% To work with grain boundaries we need some ebsd data and have to detect
% grains within the data set.

% load some example data
mtexdata twins

% detect grains
[grains,ebsd.grainId,ebsd.mis2mean] = calcGrains(ebsd('indexed'))

% smooth them
grains = grains.smooth

% visualize the grains
plot(grains,grains.meanOrientation)

%%
% Now we can extract from the grains its boundary and save it to a seperate
% variable

gB = grains.boundary

%%
% The output tells us that we have 3219 Magnesium to Magnesium boundary
% segments and 606 boundary segements where the grains are cutted by the
% scanning boundary. To restrict the grain boundaries to a specific phase
% transistion you shall do

gB_MgMg = gB('Magnesium','Magnesium')

%% Properties of grain boundaries
%
% A variable of type grain boundary contains the following properties
%
% * misorientation
% * direction
% * segLength
%
% These can be used to colorize the grain boundaries. By the following
% command we plot the grain boundaries colorized by the misorientation
% angle

plot(gB_MgMg,gB_MgMg.misorientation.angle./degree,'linewidth',2)
mtexColorbar

%%

hold on
plot(gB('notIndexed'),'lineColor','blue','linewith',5)
hold off

%%

grains.innerBoundary


function [SRE,LRE,IV,RLV,RP,LIRE,HIRE,LISRE,HISRE,LILRE,HILRE] = getSizeParams(structNum,numLevels,planC)
%function getSizeParams(structNum)
%
%This function returns zone size based features for structure structNum.
%
%APA, 03/09/2015

if numel(structNum) == 1
    if ~exist('planC')
        global planC
    end
    indexS = planC{end};
    scanNum                             = getStructureAssociatedScan(structNum,planC);
    [rasterSegments, planC, isError]    = getRasterSegments(structNum,planC);
    [mask3M, uniqueSlices]              = rasterToMask(rasterSegments, scanNum, planC);
    scanArray3M                         = getScanArray(planC{indexS.scan}(scanNum));
    SUVvals3M                           = mask3M.*double(scanArray3M(:,:,uniqueSlices));
    [minr, maxr, minc, maxc, mins, maxs]= compute_boundingbox(mask3M);
    volToEval                           = SUVvals3M(minr:maxr,minc:maxc,mins:maxs);
    volToEval(volToEval==0)             = NaN;
else
    volToEval = structNum;
end

% Quantize using percentiles
%numLevels = 16;
qtlImage = zeros(size(volToEval));
minVal = min(volToEval(:));
maxVal = max(volToEval(:));
levelsV = linspace(minVal,maxVal,numLevels+1);
%qtlsV = 0:0.1:1;
qtlsV = 1:(numLevels+1);
qltCount = 1;
for qtl = 1:length(qtlsV)-1
    %lowQtl = quantile(volToEval(:),qtlsV(qltCount));
    %hiQtl = quantile(volToEval(:),qtlsV(qltCount+1));
    lowQtl = levelsV(qltCount);
    hiQtl = levelsV(qltCount+1);
    qtlImage(volToEval > lowQtl & volToEval <= hiQtl) = qltCount;
    qltCount = qltCount + 1;
end

% Compute Zone-Size matrix
clear global run_length_matrix_all image_property
global run_length_matrix_all image_property
image_property.num_voxels = sum(~isnan(volToEval(:)));
run_length_matrix_all = zeros(max(qtlImage(:)), max(size(qtlImage)));
for idx_intensity = 1:max(qtlImage(:))
    mat_isintensity = (qtlImage==idx_intensity);
    mat_connection = bwlabeln(mat_isintensity, 26); % allow the max connectivity
    for idx_group = 1:max(mat_connection(:))
        if size(run_length_matrix_all,2)< length(find(mat_connection== idx_group))
            run_length_matrix_all(idx_intensity, length(find(mat_connection== idx_group))) = 1;
        else
            run_length_matrix_all(idx_intensity, length(find(mat_connection== idx_group))) = run_length_matrix_all(idx_intensity, length(find(mat_connection== idx_group))) +1;
        end
    end
end

% Compute scalar features
SRE = run_length_SRE();
LRE = run_length_LRE();
IV = run_length_IV();
RLV = run_length_RLV();
RP = run_length_RP();
LIRE = run_length_LIRE();
HIRE = run_length_HIRE();
LISRE = run_length_LISRE();
HISRE = run_length_HISRE();
LILRE = run_length_LILRE();
HILRE = run_length_HILRE();


% Copyright 2013 The MathWorks, Inc
%% Setup Visuals
if m == 1
    sScope = dsp.TimeScope('SampleRate',fs,...
        'TimeSpan',10/prf,'Grid',true,...
        'LayoutDimensions',[1 2],'MaximizeAxes','off', ...
        'Position',[371 646 1000 409],'NumInputPorts',2);
    set(sScope,'ActiveDisplay',2,'YLabel','Magnitude','Title','Collected Signal')
    set(sScope,'ActiveDisplay',1,'YLabel','Magnitude','Title','Transmitted Signal')
    show(sScope);
    %% Range Doppler Map
    figure('WindowStyle','docked')
    hrdmap = imagesc(sgrid,rgrid,abs(rdmap));
    xlabel('Speed (m/s)'); ylabel('Range (m)'); title('Range Doppler Map');

    %% Detection and Range Estimation
    figure('WindowStyle','docked')
    tgtrange = [NaN NaN NaN]; pmax = tgtrange;
    hold on
    for n=1:3,
        plot(sTgtMotion{n}.InitialPosition(1)*ones(2,1),[0 7e-5],'r:')
        htext(n) = text(tgtrange(n),1.05*pmax(n),int2str(tgtrange(n))); %#ok<*SAGROW>
    end
    legend('Initial Range')
    xlabel('Range (m)'); ylabel('Magnitude'); title ('Estimated Range')
    hbar = sqrt(threshold)*ones(numel(fast_time),1);
    hline = plot(range_gates,[hbar hbar]); % Threshold
    offset = numel(sMFilt.Coefficients)-1;
end

%% Stream Signals
step(sScope,abs(s),abs(rsig)); % Ctrl + A to scale axis limits
set(hrdmap,'CData',abs(fliplr(rdmap)))
drawnow
set(hline(2),'YData',[intpulses(offset:numel(fast_time)); NaN*ones(offset-1,1)])
for n=1:3,
    set(htext(n),'String',int2str(tgtrange(n)),'Position',[tgtrange(n) 1.05*pmax(n)]);
end


function [imageCha, ssim_map, metrics] = algo_BFCSA_proxA_2D_real( obj,input )
% 2D real-valued variant of FCSA with an outer Bregman Iteration
% based on Huang et al. paper on FCSA
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1.5; % > 1 suggested, otherwise it may diverge depending on "b{1,j} - FFT2D_mask(z{1,j},mask)"
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.maxitr - 5;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% maxitrOUTER = obj.maxitrOUTER;
maxitrOUTER = obj.iNOUTER;
maxitrINNER = ceil( maxitr / maxitrOUTER );
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
waveletStages = obj.trafo.waveletStages;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
z_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = real(iFFT2D(b{1,j}));
    % y{1,j} = real(FTb{1,j});
    % y{1,j+nCha} = imag(FTb{1,j});
end;
z = FTb; % starting point
b_outer = b;

x_wave = cell(1,nCha);
x_helper = x_wave;
x_wave_helper = x_wave;
x_tv = x_wave;
x_nltv = x_wave;
for j=1:nCha
    x_nltv{1,j} = 0;
end;
x_g = x_wave;
x_g_helper = x_wave;
x_g_proxA = x_wave;
y = x_wave;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec = cell(1,nCha);
    threshold = zeros(1:nCha);
    for j=1:nCha % 2*nCha
        x_wavedec{1,j} = wavedec2(z{1,j},waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
        x_wave_fine_scale = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
        threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale:end),1);
    end;
    clear x_wavedec
else
    threshold(1:nCha) = 1;
end;

threshold_wave(j) = lambdaWave * threshold(j) * 2/L;
threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
threshold_group(j) = lambdaGroup * threshold(j) * 2/L;
threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01

%% initialize metrics:
itr = 0;
metrics.xtime(itr+1)= 0;
% [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );
ssim_map = [];

%% recon

dispProgress('Proximal Average', 0, maxitrOUTER*maxitrINNER);
itr = 0;
for itrOUTER = 1:maxitrOUTER  % total iter counter
    for itrINNER = 1:maxitrINNER

        t_new = (1+sqrt(1+4*t_old^2))/2;
        t_old = t_new;

        y_old = z;   % y_old = y for complex case

        %% landweber step
        for j = 1:nCha
            x_helper{1,j} = real(iFFT2D(FFT2D_mask(z{1,j},mask))) -FTb{1,j} ;
            z{1,j} = z{1,j} - real(x_helper{1,j})/L;
        end;

        %% l1-Wavelet
        if flagWave
            for j = 1:nCha % 2*nCha
                x_wave_helper{1,j} = wavedec2(z{1,j},waveletStages,waveletFilterName_l1);
                x_wave_helper{1,j} = softthresh_real(x_wave_helper{1,j},threshold_wave(j));
                x_wave{1,j} = waverec2(x_wave_helper{1,j},waveS_l1,waveletFilterName_l1);
            end;
        end;

        %% TV
        if flagTV
            if ~flagTV_iso
                for j = 1:nCha
                    x_tv{1,j} = MTV_2D(z{1,j},threshold_TV(j),n1,n2);
                end;
            else
                for j = 1:nCha
                    if (itr < 2)
                        [x_tv{1,j}, P]=denoise_TV_One((z{1,j}), threshold_TV(j),-inf,inf,[],parsin);
                    else
                        [x_tv{1,j}, P]=denoise_TV_One((z{1,j}), threshold_TV(j),-inf,inf,P,parsin);
                    end;
                end;
            end;
        end;

        %% NLTV
        if flagNLTV
            if itr >= itrNLTV
                if flagSBNLTV
                    for j = 1:nCha
                        x_nltv{1,j} = SB_NLTVfunc_slim_rescale(z{1,j},n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                    end;
                else
                    for j = 1:nCha
                        % if mod(itr,5) == 0 || itr == 1
                        x_nltv{1,j} = NLMF(z{1,j},NLTV_struct);
                        x_nltv{1,j} = (L.*z{1,j} + 2*threshold_NLTV(j)*x_nltv{1,j})./(L+2*threshold_NLTV(j));
                        % end;
                    end;
                end;
            end;
        end;

        %% l12-Wavelet
        if flagGroup
            for j = 1:nCha
                if flag_extendImage
                    z_proxA{1,j} = extend_image(z{1,j}, waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                    x_g_helper{1,j} = wavedec2(z_proxA{1,j},waveletStages,waveletFilterName_l12);
                else
                    x_g_helper{1,j} = wavedec2(z{1,j},waveletStages,waveletFilterName_l12);
                end;

                if flag_wavetree
                    x_g_helper{2,j} = (G_prox*x_g_helper{1,j}')';
                else
                    x_g_helper{2,j} = zeros(1,size(x_g_helper{1,j},2));
                end;
            end;

            x_g_helper(1,:) = softthresh_proxA_cha(x_g_helper,threshold_group(j),nCha,waveS_l12proxA,groupnorm_index);

            for j = 1:nCha
                x_g_proxA{1,j} = waverec2(x_g_helper{1,j},waveS_l12proxA,waveletFilterName_l12);
                x_g{1,j} =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x);
            end;
        end;

        %% add prox(.)
        for j = 1:nCha
            y{1,j} = zeros(n1,n2);
            if flagWave y{1,j} = y{1,j} + x_wave{1,j}.*regularizerWeights(1); end;
            if flagTV y{1,j} = y{1,j} + x_tv{1,j}.*regularizerWeights(2); end;
            if flagGroup y{1,j} = y{1,j} + x_g{1,j}.*regularizerWeights(3); end;
            if flagNLTV y{1,j} = y{1,j} + x_nltv{1,j}.*regularizerWeights(4); end;

            if itr < itrNLTV
                y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
            else
                y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagNLTV.*regularizerWeights(4) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
            end;

            if flag_fast
                y{1,j}=y{1,j}+((t_old-1)/t_new).*(y{1,j}-y_old{1,j});
            end;
        end;

        %% metrics of current itr:
        itr = itr + 1;
%         disp(itr);
        dispProgress('Proximal Average', itr/(maxitrOUTER*maxitrINNER));

        metrics.xtime(itr+1)= toc(timer_proxA);
        for j = 1:nCha
            z{1,j} = y{1,j};
        end;
%         [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );

        if itr >= maxitr break; end;
    end;
    if itr >= maxitr break; end;
    for j=1:nCha
        b_outer{1,j} = b_outer{1,j} + b{1,j} - FFT2D_mask(z{1,j},mask);
        FTb{1,j} = real(iFFT2D(b_outer{1,j}));
    end;
end;
dispProgress('Proximal Average', 'Close');

imageCha = z;
for j = 1:nCha
    imageCha{1,j} = turn_image( imageCha{1,j} );
end;
% for j = 1:nCha+1
%     ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%     ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
% end;

end