% prune isolated, noisy neurons

f_r2d = @(r) sqrt(2 * (1-r));
f_d2r = @(d) 1- d.^2 / 2;
f_2r = @(r) f_d2r(f_r2d(r) * 2);

%%
FishClusterList = {11, 'all_thr50'};

stimCase = 2;
flag_truncate_after_transition = 1;

rx_f = 0.85;
q_f =  1/3; % final quantile

iFish = FishClusterList{1};
clusterName = FishClusterList{2};
if getappdata(hfig, 'i_fish') ~= iFish
    f.LoadFullFish(hfig,iFish);
end

[cIXt,gIXt,numKt]=load_cluster(hfig, clusterName);
cgk0 = {cIXt,gIXt,numKt};
cgk_current = cgk0;
push_cgk(hfig, cgk_current);

setStimRange(hfig, f, s2stimStr(stimCase));
if flag_truncate_after_transition == 1
    updateTRange_remove_transition(hfig,f);  % truncate after transition
end

%% step 1, norm prune
p_var = 0.95;
norm_th = 0.85;

tic
display('step 1: prune by norm (PCA)');
[cgk1, cgk1_rest] = cgk_norm_prune(hfig, norm_th);
toc

push_cgk(hfig, cgk1);
% push_cgk(hfig, cgk1_rest);

cgk_current = cgk1;

%% step 2, prune isolated neurons
p_var = 0.95;
% rx2 = f_d2r(f_r2d(rx_f) * sqrt(2)) % triangle inequality (conservative)
rx2 = rx_f .^ 2 % high dim asymptotic
nc_th = 8;
nc_bin = 1e3;
niter = 20;

tic
display('step2, prune isolated neurons');

[cgk2, cgk2_rest] = cgk_isolated_prune_dist(hfig, rx2, nc_th, niter);

push_cgk_k10(hfig, cgk2_rest);

push_cgk_k10(hfig, cgk2);
cgk_current = cgk2;

%% step 3-pre, parameter search
ncutoff = 4;
qx = q_f;
nc_th = 8;
niter = 20;

flag_linkage_method = 'complete';

% rx2 = f_d2r(f_r2d(rx_f) * 2) % center rcos threshold
% rx2 = rx_f .^ 2 % high dim asymptotic
d1 = 1 - f_d2r(f_r2d(rx_f))
d2 = min(2, 1 - f_d2r(f_r2d(rx_f) * 4))
rx = rx_f;
cutoff_dist_ls = linspace(d1, d2, ncutoff);
nC_prune_ls = zeros(1, ncutoff);
ng_ls = zeros(1, ncutoff);
ng0_ls = zeros(1, ncutoff); % raw result from linkage
cgk3_ls = cell(1, ncutoff);
cgk3_rest_ls = cell(1, ncutoff);

M = getappdata(hfig, 'M');
[nC, nT] = size(M);
M = zscore(M, 0, 2);
[pca_dim_M, k_var_M, U_M, S_M, V_M] = PCA_MP_fit(M, p_var, 0);

Mp = U_M(:, 1:k_var_M) * S_M(1:k_var_M, 1:k_var_M);
nTp = size(Mp, 2);

tic
display('linkage parameter search')
for i_c = 1 : ncutoff
    cutoff_dist = cutoff_dist_ls(i_c);
    switch flag_linkage_method
        case 'center'
            Z_link = linkage_cent_cos_v2(Mp);
        case 'average'
            Z_link = linkage(Mp, 'average', 'cosine');
        case  'single'
            Z_link = linkage(Mp, 'single', 'cosine');
        case  'complete'
            Z_link = linkage(Mp, 'complete', 'cosine');
        case 'centroid'
            norm_Mp = normM(Mp);
            Mp_norm = Mp ./ repmat(norm_Mp, [1, nTp]);
            %         d_scale = 2 * median(norm_Mp);
            %         cutoff_dist = d_scale * cutoff_dist;
%             cutoff_dist = sqrt(2 * cutoff_dist);
            Z_link = linkage(Mp_norm, 'centroid', 'euclidean');
    end
c_link_temp = cluster_u(Z_link, 'cutoff', cutoff_dist, 'criterion', 'distance');
cgk_current_temp = cgk_current;
cgk_current_temp{2} = c_link_temp;
cgk_current_temp{3} = max(c_link_temp); % not yet pushed to GUI

[cgk3_ls{i_c}, cgk3_rest_ls{i_c}] = cgkMp_rx_qx_prune(cgk_current_temp, Mp, rx, qx, nc_th, niter);

nC_prune_ls(i_c) = length(cgk3_rest_ls{i_c}{1});
ng0_ls(i_c) = length(unique(cgk_current_temp{2}));
ng_ls(i_c) = length(unique(cgk3_ls{i_c}{2}));
end
toc

%
x_ls = cutoff_dist_ls;
figure;
subplot(211);
plot(x_ls, ng0_ls, 'bo-');
hold on;
plot(x_ls, ng_ls, 'ro-');
hold off;
legend({'linkage', 'pruned'});
title('number of clusteres')
subplot(212);
plot(x_ls, nC_prune_ls, 'bo-');
title('pruned cells')

%%
save(fullfile(working_data_dir, 'step_prune_data_temp_01.mat'), 'cgk2', 'cgk3_ls', 'cgk3_rest_ls');

% %% step 3, linkage clustering
% p_var = 0.95;
% rx2 = f_d2r(f_r2d(rx_0) * 2) % center rcos threshold
% rx = rx_f;
% qx = q_f;
% nc_th = 8;
% niter = 20;
%
% cutoff_dist = 1 - rx2;
% flag_linkage_method = 'complete';
%
% tic
% display('step3, complete linkage');
%
% M = getappdata(hfig, 'M');
% [nC, nT] = size(M);
% M = zscore(M, 0, 2);
% [pca_dim_M, k_var_M, U_M, S_M, V_M] = PCA_MP_fit(M, p_var, 0);
%
% Mp = U_M(:, 1:k_var_M) * S_M(1:k_var_M, 1:k_var_M);
% nTp = size(Mp, 2);
%
%
% switch flag_linkage_method
%         case 'center'
%             Z_link = linkage_cent_cos_v2(Mp);
%         case 'average'
%             Z_link = linkage(Mp, 'average', 'cosine');
%         case  'single'
%             Z_link = linkage(Mp, 'single', 'cosine');
%         case  'complete'
%             Z_link = linkage(Mp, 'complete', 'cosine');
%         case 'centroid'
%             norm_Mp = normM(Mp);
%             Mp_norm = Mp ./ repmat(norm_Mp, [1, nTp]);
%             %         d_scale = 2 * median(norm_Mp);
%             %         cutoff_dist = d_scale * cutoff_dist;
% %             cutoff_dist = sqrt(2 * cutoff_dist);
%             Z_link = linkage(Mp_norm, 'centroid', 'euclidean');
% end
% c_link = cluster_u(Z_link, 'cutoff', cutoff_dist, 'criterion', 'distance');
% cgk_current{2} = c_link;
% cgk_current{3} = max(c_link); % not yet pushed to GUI
%
% [cgk3, cgk3_rest] = cgkMp_rx_qx_prune(cgk_current, Mp, rx, qx, nc_th, niter);
%
%
% push_cgk_k10(hfig, cgk3_rest);
%
% push_cgk(hfig, cgk3);
% cgk_current = cgk3;

%%

%
% %%
% push_cgk(hfig, cgk_link);
% rx = 0.9;
% rq = 0.30;
% nsize_th = 5;
%
% M = getappdata(hfig, 'M');
% M = zscore(M, 0, 2);
% [nC, nT] = size(M);
% p_var = 0.95;
% flag_plot = 0;
% [pca_dim_M, k_var_M, U_M, S_M, V_M] = PCA_MP_fit(M, p_var, flag_plot);
% Mp = U_M(:, 1:k_var_M) * S_M(1:k_var_M, 1:k_var_M);
% nTp = size(Mp, 2);
%
% gls = sort(unique(cgk_link{2}), 'ascend');
% ngc = length(gls);
% tf_core = zeros(nC, 1);
% for i = 1 : ngc
%     tf_gc_temp = cgk_link{2} == gls(i);
%     id_gc_temp = find(tf_gc_temp);
%     mM_temp = mean(Mp(tf_gc_temp, :), 1);
%     rc_temp = rcosM(Mp(tf_gc_temp, :), mM_temp);
%     rc_temp_sort = sort(rc_temp, 'descend');
%     i_th_temp = round(rq * length(rc_temp) + 1);
%     i_th_temp = max(i_th_temp, 1);
%     i_th_temp = min(i_th_temp, length(rc_temp));
%     rq_th = rc_temp_sort(i_th_temp);
%     if i_th_temp >= nsize_th
%         r_th_temp = max(rx, rq_th);
%     else
%         r_th_temp = rx;
%     end
%     if sum(rc_temp > r_th_temp) >= nsize_th
%         tf_core(id_gc_temp(rc_temp > rx)) = 1;
%     end
% end
%
% tf_core = logical(tf_core);
% cgk_core = cgk_link;
% cgk_core{1} = cgk_core{1}(tf_core);
% cgk_core{2} = cgk_core{2}(tf_core);
%
% push_cgk(hfig, cgk_core);

% %%
%
%
% ns=length(s);
% % plot eigenvalues
% figure;
% set(gcf,'position',[1117         536         1120         420]);
% subplot(1,3,1);
% scatter(1:ns,s,'ro');
% title('eigenvalues')
%
% subplot(1,3,2);
% scatter(1:ns,log(s),'ro');
% title('log eigenvalues')
%
% sum_res=flipud(cumsum(flipud(s.^2)));
% total_var=sum_res(1);
% sum_res=[sum_res(2:end);0]/total_var;
% subplot(1,3,3);
% scatter(1:ns,sum_res*100,'ro');
% title('residue variance (percentage)')
%
% nX=4;
% X=V(:,1:nX);
% Y=X;
% % for i=1:nC
% %     Y(i,:)=Y(i,:)/norm(X(i,:),2);
% % end
%
% markerSize=16;
%
% u1=Y(:,1);
% u2=Y(:,2);
% u3=Y(:,3);
%
% if ismember(2,dims)
%     figure;
%     set(gcf,'position',[1131   43   560   420]);
%     scatter(u1,u2,markerSize*ones(1,nC), cmap(gIX,:),'fill');
%     axis equal;
% end;
%
% if ismember(3,dims)
%     figure;
%     set(gcf,'position',[87   386   733   548]);
%     scatter3(u1,u2,u3,markerSize*ones(1,nC), cmap(gIX,:),'fill');
%     axis equal;
% end;
%
%
% %%
% nkmean = 5;
% idx = kmeans(Y, nkmean, 'replicate', 20);
%
% cmap_kmean = cmap_cluster(hfig, nkmean);
%
% if ismember(2,dims)
%     figure;
%     set(gcf,'position',[1131   43   560   420]);
%     scatter(u1,u2,markerSize*ones(1,nC), cmap_kmean(idx,:),'fill');
%     axis equal;
% end;
%
% if ismember(3,dims)
%     figure;
%     set(gcf,'position',[87   386   733   548]);
%     scatter3(u1,u2,u3,markerSize*ones(1,nC), cmap_kmean(idx,:),'fill');
%     axis equal;
% end;
%
% % cgk0_temp = get_cgk(hfig);
% cgk1_temp = cgk0_temp;
% cgk1_temp{2} = idx;
% cgk1_temp{3} = max(idx);
% push_cgk(hfig, cgk1_temp);

%%
% cgk_s12_diff = cgk_s1;
% [cgk_s1{1}, id_sort_temp] = sort(cgk_s1{1});
% cgk_s1{2} = cgk_s1{2}(id_sort_temp);
% [cgk_s2{1}, id_sort_temp] = sort(cgk_s2{1});
% cgk_s2{2} = cgk_s2{2}(id_sort_temp);
%
% tf_diff = cgk_s1{2} ~= cgk_s2{2};
% cgk_s12_diff{1} = cgk_s12_diff{1}(tf_diff);
% cgk_s12_diff{2} = cgk_s12_diff{2}(tf_diff);
%
% push_cgk(hfig, cgk_s12_diff);

% %% parameter  search of inconsistency
% ncutoff = 60;
% q_f =  1/3; % final quantile
% cutoff_dist_ls = linspace(0.2, 1, ncutoff);
% nC_rest1_ls = zeros(1, ncutoff);
% ngc1_ls = zeros(1, ncutoff);
% nC_rest2_ls = zeros(1, ncutoff);
% ngc2_ls = zeros(1, ncutoff);
% cgk_link1_ls = cell(1, ncutoff);
% cgk_link2_ls = cell(1, ncutoff);
% cgk_link1_rest_ls = cell(1, ncutoff);
% cgk_link2_rest_ls = cell(1, ncutoff);
%
% tic
% display('linkage clustering')
% for i_p = 1 : ncutoff
%     cutoff_dist = cutoff_dist_ls(i_p);
%     nc_size_th = nc_th_f;
%     p_var = 0.95;
% %     flag_linkage_method = 'average';
%     flag_linkage_method = 'centroid';
%     % flag_linkage_method = 'center';
%     % flag_linkage_method = 'complete';
%
%     push_cgk(hfig, cgk3_temp);
%     M = getappdata(hfig, 'M');
%     M = zscore(M, 0, 2);
%     [nC, nT] = size(M);
%
%     [pca_dim_M, k_var_M, U_M, S_M, V_M] = PCA_MP_fit(M, p_var, 0);
%     Mp = U_M(:, 1:k_var_M) * S_M(1:k_var_M, 1:k_var_M);
%     nTp = size(Mp, 2);
%
%     switch flag_linkage_method
%         case 'center'
%             Z_link = linkage_cent_cos_v2(Mp);
%         case 'average'
%             Z_link = linkage(Mp, 'average', 'cosine');
%         case  'single'
%             Z_link = linkage(Mp, 'single', 'cosine');
%         case  'complete'
%             Z_link = linkage(Mp, 'complete', 'cosine');
%         case 'centroid'
%             norm_Mp = normM(Mp);
%             Mp_norm = Mp ./ repmat(norm_Mp, [1, nTp]);
%             %         d_scale = 2 * median(norm_Mp);
%             %         cutoff_dist = d_scale * cutoff_dist;
%             cutoff_dist = sqrt(2 * cutoff_dist);
%             Z_link = linkage(Mp_norm, 'centroid', 'euclidean');
%     end
%     % c_link = cluster(Z_link, 'cutoff', cutoff_dist, 'criterion', 'distance');
%     c_link = cluster_u(Z_link, 'cutoff', cutoff_dist, 'criterion', 'inconsistent');
%
%     nc_link = max(c_link);
%     nsize_c_link = zeros(nc_link, 1);
%     for i = 1 : nc_link
%         nsize_c_link(i) = sum(c_link == i);
%     end
%     id_c_size_th = find(nsize_c_link >= nc_size_th);
%     nid_c = length(id_c_size_th);
%
%     % update loop parameter
%     ngc1_ls(i_p) = nid_c;
%     nC_rest1_ls(i_p) = length(cgk3_temp{1}) - sum(nsize_c_link(id_c_size_th));
%
%
%     Mcent_link = zeros(nid_c, nTp);
%     nsize_c_link_new = zeros(nid_c, 1);
%     for i = 1 : nid_c
%         Mcent_link(i, :) = mean(Mp(c_link == id_c_size_th(i), :), 1);
%         nsize_c_link_new(i) = sum(c_link == id_c_size_th(i));
%     end
%
%     if size(Mcent_link, 1) > 1
%         switch flag_linkage_method
%             case 'center'
%                 D_temp = pdist(Mcent_link, 'cosine');
%                 tree_temp = linkage_cent_cos_v2(Mcent_link, nsize_c_link_new);
%             case 'average'
%                 D_temp = pdist(Mcent_link, 'cosine');
%                 tree_temp = linkage(Mcent_link, 'average', 'cosine');
%             case  'single'
%                 D_temp = pdist(Mcent_link, 'cosine');
%                 tree_temp = linkage(Mcent_link, 'single', 'cosine');
%             case  'complete'
%                 D_temp = pdist(Mcent_link, 'cosine');
%                 tree_temp = linkage(Mcent_link, 'complete', 'cosine');
%             case 'centroid'
%                 D_temp = pdist(Mcent_link, 'euclidean');
%                 tree_temp = linkage(Mcent_link, 'centroid', 'euclidean');
%         end
%         leafOrder_temp = optimalleaforder(tree_temp, D_temp);
%     else
%         leafOrder_temp = 1;
%     end
%
%     cgk_link_rest = cgk3_temp;
%     cgk_link = cgk3_temp;
%     cgk_link{2}(:) = 0;
%     for i = 1 : nid_c
%         cgk_link{2}(c_link == id_c_size_th(leafOrder_temp(i))) = i;
%     end
%     cgk_link_rest{1} = cgk_link_rest{1}(cgk_link{2} == 0);
%     cgk_link_rest{2} = cgk_link_rest{2}(cgk_link{2} == 0);
%     cgk_link{1} = cgk_link{1}(cgk_link{2} > 0);
%     cgk_link{2} = cgk_link{2}(cgk_link{2} > 0);
%     cgk_link{3} = nid_c;
%
%     % update loop parameter
%     cgk_link1_ls{i_p} = cgk_link;
%     cgk_link1_rest_ls{i_p} = cgk_link_rest;
%
%     if length(cgk_link{1}) > 1
%         % core selection
%         push_cgk(hfig, cgk_link);
%         M = getappdata(hfig, 'M');
%         M = zscore(M, 0, 2);
%         [nC, nT] = size(M);
%         [pca_dim_M, k_var_M, U_M, S_M, V_M] = PCA_MP_fit(M, p_var, 0);
%         Mp = U_M(:, 1:k_var_M) * S_M(1:k_var_M, 1:k_var_M);
%         nTp = size(Mp, 2);
%
%         nc_link = cgk_link{3};
%         nsize_c_link = zeros(nc_link, 1);
%         tf_core = false(length(cgk_link{1}), 1);
%         for i = 1 : nc_link
%             mM_temp = mean(Mp(cgk_link{2} == i, :), 1);
%             id_cluster_temp = find(cgk_link{2} == i);
%             rcos_v_temp = rcosM(Mp(cgk_link{2} == i, :), mM_temp);
%             r_sort_temp = sort(rcos_v_temp, 'descend');
%             i_th_temp = round(q_f * length(r_sort_temp) + 1);
%             i_th_temp = max(i_th_temp, 1);
%             i_th_temp = min(i_th_temp, length(r_sort_temp));
%             rq_th = r_sort_temp(i_th_temp);
%             if i_th_temp >= nc_th_f
%                 r_th_temp = max(rq_th, rx_f);
%             else
%                 r_th_temp = rx_f;
%             end
%             nsize_c_link(i) = sum(rcos_v_temp > r_th_temp);
%             if nsize_c_link(i) >= nc_th_f
%                 tf_core(id_cluster_temp(rcos_v_temp > r_th_temp)) = true;
%             end
%         end
%         id_c_size_th = find(nsize_c_link >= nc_th_f);
%         nid_c = length(id_c_size_th);
%         Mcent_link = zeros(nid_c, nTp);
%         for i = 1 : nid_c
%             tf_valid_temp = cgk_link{2} == id_c_size_th(i) & tf_core;
%             Mcent_link(i, :) = mean(Mp(tf_valid_temp, :), 1);
%         end
%         if size(Mcent_link, 1) > 1
%             flag_linkage_method_step2 = 'average';
%             switch flag_linkage_method_step2
%                 case 'center'
%                     D_temp = pdist(Mcent_link, 'cosine');
%                     tree_temp = linkage_cent_cos_v2(Mcent_link, nsize_c_link_new);
%                 case 'average'
%                     D_temp = pdist(Mcent_link, 'cosine');
%                     tree_temp = linkage(Mcent_link, 'average', 'cosine');
%                 case  'single'
%                     D_temp = pdist(Mcent_link, 'cosine');
%                     tree_temp = linkage(Mcent_link, 'single', 'cosine');
%                 case  'complete'
%                     D_temp = pdist(Mcent_link, 'cosine');
%                     tree_temp = linkage(Mcent_link, 'complete', 'cosine');
%                 case 'centroid'
%                     D_temp = pdist(Mcent_link, 'euclidean');
%                     tree_temp = linkage(Mcent_link, 'centroid', 'euclidean');
%             end
%             leafOrder_temp = optimalleaforder(tree_temp, D_temp);
%         else
%             leafOrder_temp = 1;
%         end
%
%         cgk_link2_rest = cgk_link;
%         cgk_link2 = cgk_link;
%         cgk_link2{2}(:) = 0;
%         for i = 1 : nid_c
%             cgk_link2{2}(cgk_link{2} == id_c_size_th(leafOrder_temp(i))) = i;
%         end
%         cgk_link2_rest{1} = cgk_link2_rest{1}(cgk_link2{2} == 0);
%         cgk_link2_rest{2} = cgk_link2_rest{2}(cgk_link2{2} == 0);
%         cgk_link2{1} = cgk_link2{1}(cgk_link2{2} > 0);
%         cgk_link2{2} = cgk_link2{2}(cgk_link2{2} > 0);
%         cgk_link2{3} = nid_c;
%
%         % update loop parameter
%         cgk_link2_ls{i_p} = cgk_link2;
%         cgk_link2_rest_ls{i_p} = cgk_link2_rest;
%
%         ngc2_ls(i_p) = nid_c;
%         nC_rest2_ls(i_p) = length(cgk_link{1}) - sum(nsize_c_link(id_c_size_th));
%     else
%         % update loop parameter
%         cgk_link2_ls{i_p} = nan;
%         cgk_link2_rest_ls{i_p} = nan;
%
%         ngc2_ls(i_p) = 0;
%         nC_rest2_ls(i_p) = 0;
%     end
% end
%
% toc
%
% %
% figure;
% subplot(211);
% plot(cutoff_dist_ls, ngc1_ls, 'bo-');
% hold on;
% plot(cutoff_dist_ls, ngc2_ls, 'ro-');
% hold off;
% title('number of clusteres')
% subplot(212);
% plot(cutoff_dist_ls, nC_rest1_ls, 'bo-');
% hold on;
% plot(cutoff_dist_ls, nC_rest2_ls, 'ro-');
% hold off;
% title('pruned cells')
%
%
% beep
% beep
%
% %%
% push_cgk(hfig, cgk_link2_ls{35});


function dvh = matRad_calcDVH(cst,doseCube,dvhType,doseGrid)
% matRad dvh calculation
%
% call
%   dvh = matRad_calcDVH(cst,doseCube)
%   dvh = matRad_calcDVH(cst,doseCube,dvhType)
%   dvh = matRad_calcDVH(cst,doseCube,doseGrid)
%   dvh = matRad_calcDVH(cst,doseCube,dvhType,doseGrid)
%
% input
%   cst:        matRad cst struct
%   doseCube:   arbitrary doseCube (e.g. physicalDose)
%   dvhType:    (optional) string, 'cum' for cumulative, 'diff' for differential
%               dvh
%   doseGrid:   (optional) use predefined evaluation points. Useful when
%               comparing multiple realizations
%
% output
%   dose volume histogram
%
% References
%   van't Riet et. al., IJROBP, 1997 Feb 1;37(3):731-6.
%   Kataria et. al., J Med Phys. 2012 Oct-Dec; 37(4): 207�213.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2016 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('dvhType','var') || isempty(dvhType)
    dvhType = 'cum';
end

if ~exist('doseGrid', 'var') || isempty(doseGrid)
    maxDose = max(doseCube(:));
    minDose = min(doseCube(:));

    % get dvhPoints for every structure and every scenario the same
    n = 1000;
    if strcmp(dvhType, 'cum')
        doseGrid = linspace(0,maxDose*1.05,n);
    elseif strcmp(dvhType, 'diff')
        doseGrid = linspace(0.95*minDose,maxDose*1.05,n);
    end
end

numOfVois = size(cst,1);
dvh = struct;
for i = 1:numOfVois
    dvh(i).doseGrid     = doseGrid;
    dvh(i).volumePoints = getDVHPoints(cst, i, doseCube, doseGrid, dvhType);
    dvh(i).name         = cst{i,2};
end

end %eof

function dvh = getDVHPoints(cst, sIx, doseCube, dvhPoints, dvhType)
n = numel(dvhPoints);
dvh         = NaN * ones(1,n);
indices     = cst{sIx,4}{1};
numOfVoxels = numel(indices);

doseInVoi   = doseCube(indices);

switch dvhType
    case 'cum' % cummulative DVH
        for j = 1:n
            dvh(j) = sum(doseInVoi >= dvhPoints(j));
        end

    case 'diff' % differential DVH
        binning = (dvhPoints(2) - dvhPoints(1))/2;
        for j = 1:n % differential DVH
            dvh(j) = sum(dvhPoints(j) + binning > doseInVoi & doseInVoi > dvhPoints(j) - binning);
        end

end
dvh = dvh ./ numOfVoxels * 100;
end %eof getDVHPoints


img = rgb2gray(im2double(imread('corner2.gif')));
[map r c] = susanCorner(img);
figure,imshow(img),hold on
plot(c,r,'o')


function [post,model] = spm_vb_roi_anova (VOI_fname,SPM,factor)
% Bayesian ANOVA for a region of interest
% FORMAT [post,model] = spm_vb_roi_anova (VOI_fname,SPM,factor)
%
% VOI_fname   - VOI filename
% SPM         - SPM data structure
% factor      - data structure relating conditions to levels of factors
%
% model       - data structure describing models
%               (m).F             model evidence
%               (m).X             design matrix
% post        - Posterior probabilities of
%               .factor1        main effect of factor 1
%               .factor2        main effect of factor 2
%               .interaction    interaction
%               .average        average
%__________________________________________________________________________
% Copyright (C) 2005-2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_vb_roi_anova.m 6079 2014-06-30 18:25:37Z spm $

if nargin < 2
    [Pf, sts] = spm_select(1,'^SPM\.mat$','Select SPM.mat');
    if ~sts, return; end
    swd = spm_file(Pf,'fpath');
    load(fullfile(swd,'SPM.mat'))
end

load(VOI_fname);

Y    = xY.y;
xyz  = xY.XYZmm;
N    = size(xyz,2);
M    = diag(SPM.xVol.M);
m    = 1./abs(M(1:3));
xyz  = (m*ones(1,N)).*xyz;
vxyz = spm_vb_neighbors(xyz',1);

%-Set number of AR coefficients
%--------------------------------------------------------------------------
try
    SPM.PPM.AR_P;
catch
    SPM.PPM.AR_P = 0;
end

%-Specify type of prior for regression coefficients
%--------------------------------------------------------------------------
try
    SPM.PPM.priors.W;
catch
    if N==1
        SPM.PPM.priors.W = 'Voxel - Shrinkage';
    else
        SPM.PPM.priors.W = 'Spatial - LORETA';
    end
end

%-Specify type of prior for AR coefficients
%--------------------------------------------------------------------------
try
    SPM.PPM.priors.A;
catch
    if N==1
        SPM.PPM.priors.A = 'Voxel - Shrinkage';
    else
        SPM.PPM.priors.A = 'Spatial - LORETA';
    end
end

% Get matrices that will remove low-frequency drifts
% if high pass filters have been specified
%--------------------------------------------------------------------------
s=1;
sess_nScan=length(SPM.xX.K(s).row);
if size(SPM.xX.K(s).X0,2) > 0
    X0=SPM.xX.K(s).X0;
    hpf(s).R0=eye(sess_nScan)-X0*pinv(X0);
else
    hpf(s).R0=eye(sess_nScan);
end

%-Filter data to remove low frequencies
R0Y = hpf(s).R0*Y(SPM.Sess(s).row,:);

%-Set optimisation parameters
%--------------------------------------------------------------------------
try
    SPM.PPM.maxits;
catch
    SPM.PPM.maxits = 16;
end
try
    SPM.PPM.tol;
catch
    SPM.PPM.tol = 0.00001;
end

%-Specify basis functions
%--------------------------------------------------------------------------
% SPM.xBF.name='hrf';
% SPM.xBF.order=1;
SPM.xBF.name='hrf (with time derivative)';
SPM.xBF.order=2;
SPM.xBF.length=32;
SPM.xBF = spm_get_bf(SPM.xBF);

nf=length(factor);

model = spm_vb_models (SPM,factor);

original_SPM = SPM;

%-Fit models
%--------------------------------------------------------------------------
for m=1:6

    if nf==2 || (nf==1 && (m==1 || m==2 || m==6))
        % fit model
        SPM = original_SPM;

        if ~(m==1 || m==6)
            % Get design matrix for relevant input set
            SPM.Sess(1).U=model(m).U;
            SPM.Sess(1).U=spm_get_ons(SPM,1);
            SPM=spm_fmri_design(SPM,0);     % Don't write SPM.mat file
            model(m).X=SPM.xX.X;
        end
        slice = spm_vb_init_volume (model(m).X,SPM.PPM.AR_P);

        slice.maxits=SPM.PPM.maxits;
        slice.tol=SPM.PPM.tol;
        slice.compute_det_D=1;
        slice.verbose=1;
        slice.update_w=1;
        slice.update_lambda=1;
        slice.update_F=1;
        slice = spm_vb_set_priors(slice,SPM.PPM.priors,vxyz);
        slice = spm_vb_glmar(R0Y,slice);

        model(m).F=slice.F;

        model(m).slice=slice;
    end
end

if nf==2

    F=[model(3).F,model(2).F];
    F=F-mean(F);
    post.factor1=exp(F(1))/sum(exp(F));

    F=[model(4).F,model(2).F];
    F=F-mean(F);
    post.factor2=exp(F(1))/sum(exp(F));

    F=[model(6).F,model(5).F];
    F=F-mean(F);
    post.interaction=exp(F(1))/sum(exp(F));

    F=[model(2).F,model(1).F];
    F=F-mean(F);
    post.average=exp(F(1))/sum(exp(F));

elseif nf==1

    F=[model(2).F,model(1).F];
    F=F-mean(F);
    post.average=exp(F(1))/sum(exp(F));

    F=[model(6).F,model(2).F];
    F=F-mean(F);
    post.factor1=exp(F(1))/sum(exp(F));

end


function p = prior_invunif(varargin)
%PRIOR_INVUNIF  Uniform prior structure for the inverse of the parameter
%
%  Description
%    P = PRIOR_INVUNIF creates uniform prior structure for the
%    inverse of the parameter.
%
%  See also
%    PRIOR_*
%
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010,2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_INVUNIFORM';
  ip.addOptional('p', [], @isstruct);
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'Inv-Uniform';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'Inv-Uniform')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  if init
    % set functions
    p.fh.pak = @prior_invunif_pak;
    p.fh.unpak = @prior_invunif_unpak;
    p.fh.lp = @prior_invunif_lp;
    p.fh.lpg = @prior_invunif_lpg;
    p.fh.recappend = @prior_invunif_recappend;
  end

end

function [w, s, h] = prior_invunif_pak(p, w)
  w=[];
  s={};
  h=[];
end

function [p, w] = prior_invunif_unpak(p, w)
  w = w;
  p = p;
end

function lp = prior_invunif_lp(x, p)
  lJ=-log(x)*2;   % log(1/x^2) log(|J|) of transformation
  lp = sum(0 +lJ);
end

function lpg = prior_invunif_lpg(x, p)
  lJg=-2./x;      % gradient of log(|J|) of transformation
  lpg = zeros(size(x)) + lJg;
end

function rec = prior_invunif_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
end


classdef FEMPlotter < handle

    properties (Access = private)
        dim
        mesh
        displacement
        dispCoords
    end

    methods (Access = public)

        function obj = FEMPlotter(cParams)
            obj.init(cParams);
        end

        function plot(obj)
            ndimf = obj.dim.ndimField;
            switch ndimf
                case 2
                    obj.plotFem2D();
                case 3
                    obj.plotFem3D();
            end
        end

    end

    methods (Access = private)

        function init(obj, cParams)
            obj.dim          = cParams.dim;
            obj.mesh         = cParams.mesh;
            obj.displacement = cParams.displacement;
            obj.dispCoords   = obj.computeDisplacedCoords();
        end

        function dispCoords = computeDisplacedCoords(obj)
            ndimf = obj.dim.ndimField;
            ndof = obj.dim.ndof;
            coords = zeros(ndof,1);
            for i = 1:ndimf
                dofs = i:ndimf:ndof;
                coor = obj.mesh.coord(:,i);
                coords(dofs) = coor;
            end
            delta = obj.displacement;
            dispCoords = coords + delta;
        end

        function plotFem2D(obj)
            obj.plotNodes2D();
            obj.plotDisplacement2D();
        end

        function plotFem3D(obj)
            obj.plotNodes3D();
            obj.plotDisplacement3D();
        end

        function plotNodes2D(obj)
            Tn = obj.mesh.connec;
            x  = obj.mesh.coord(:,1);
            y  = obj.mesh.coord(:,2);
            figure()
            hold on
            colormap jet;
            plot(x(Tn)',y(Tn)','--','linewidth',0.5);
        end

        function plotDisplacement2D(obj)
            ndimf  = obj.dim.ndimField;
            ndof   = obj.dim.ndof;
            coords = obj.dispCoords;
            x = coords(1:ndimf:ndof);
            y = coords(2:ndimf:ndof);
            Tn   = obj.mesh.connec;
            plot(x(Tn)',y(Tn)','-k','linewidth',0.5);
        end

        function plotNodes3D(obj)
            Tn = obj.mesh.connec;
            x  = obj.mesh.coord(:,1);
            y  = obj.mesh.coord(:,2);
            z  = obj.mesh.coord(:,3);
            figure()
            hold on
            colormap jet;
            plot3(x(Tn)',y(Tn)',z(Tn)','--','linewidth',0.5);
        end

        function plotDisplacement3D(obj)
            ndimf  = obj.dim.ndimf;
            ndof   = obj.dim.ndof;
            coords = obj.dispCoords;
            x = coords(1:ndimf:ndof);
            y = coords(2:ndimf:ndof);
            z = coords(3:ndimf:ndof);
            Tn   = obj.mesh.connec;
            plot3(x(Tn)',y(Tn)',z(Tn)','-k','linewidth',0.5);
        end

    end
end


function cnn_toy_data_generator(dataDir)
%CNN_TOY_DATA_GENERATOR
%   Generates toy data in the given path: random image of triangles,
%   squares and circles.
%
%   The directory format is: '<dataDir>/<set>/<label>/<sample>.png', where
%   <set> is 'train' or 'val', <label> is an integer between 1 and 3, and
%   <sample> is the sample index.

% Copyright (C) 2017 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

  % Set random seed
  rng('default') ;
  rng(0) ;

  % The sets, and number of samples per label in each set
  sets = {'train', 'val'} ;
  numSamples = [1500, 150] ;

  % Number of polygon points in each class. The circle is created with 50
  % points.
  numPoints = [3, 4, 50] ;

  for s = 1:2  % Iterate sets
    for label = 1:3  % Iterate labels
      fprintf('Generating images for set %s, label %i...\n', sets{s}, label) ;

      mkdir(sprintf('%s/%s/%i', dataDir, sets{s}, label)) ;

      for i = 1:numSamples(s)  % Iterate samples
        % Points of a regular polygon, with random rotation and scale
        radius = randi([11, 14]) ;
        angles = rand(1) * 2 * pi + (0 : 2 * pi / numPoints(label) : 2 * pi) ;
        xs = 16.5 + cos(angles) * radius ;
        ys = 16.5 + sin(angles) * radius ;

        % Generate image
        image = poly2mask(xs, ys, 32, 32) ;

        % Save it
        imwrite(image, sprintf('%s/%s/%i/%04i.png', dataDir, sets{s}, label, i)) ;
      end
    end
  end

end


%% DEMO_febio_0002_beam_force
% Below is a demonstration for:
%
% * Building geometry for a beam with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 4.0
% * febio, FEBio
% * beam force loading
% * force control boundary condition
% * hexahedral elements, hex8, hex20
% * beam, rectangular
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=20;
faceAlpha1=0.8;
markerSize=40;
markerSize2=20;
lineWidth=3;

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement

%Specifying dimensions and number of elements
beamWidth=10;
sampleWidth=beamWidth; %Width
sampleThickness=4*beamWidth; %Thickness
sampleHeight=beamWidth; %Height
pointSpacings=3*ones(1,3); %Desired point spacing between nodes
numElementsWidth=round(sampleWidth/pointSpacings(1)); %Number of elemens in dir 1
numElementsThickness=round(sampleThickness/pointSpacings(2)); %Number of elemens in dir 2
numElementsHeight=round(sampleHeight/pointSpacings(3)); %Number of elemens in dir 3

elementType='hex20'; %'hex8' or 'hex20'

%Define applied force
appliedForce=[0 0 -2e-3];

%Material parameter set
c1=1e-3; %Shear-modulus-like parameter
m1=8; %Material parameter setting degree of non-linearity
k_factor=1e2; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size

runMode='external';% 'internal' or 'external'

%% Creating model geometry and mesh
% A box is created with tri-linear hexahedral (hex8) elements using the
% |hexMeshBox| function. The function offers the boundary faces with
% seperate labels for the top, bottom, left, right, front, and back sides.
% As such these can be used to define boundary conditions on the exterior.

% Create a box with hexahedral elements
beamDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
beamElementNumbers=[numElementsWidth numElementsThickness numElementsHeight]; %Number of elements
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(beamDimensions,beamElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
E=meshStruct.elements; %The elements
V=meshStruct.nodes; %The nodes (vertices)
Fb=meshStruct.facesBoundary; %The boundary faces
Cb=meshStruct.boundaryMarker; %The "colors" or labels for the boundary faces
elementMaterialIndices=ones(size(E,1),1); %Element material indices

if strcmp(elementType,'hex20')
    [E,V,~,Fb]=hex8_hex20(E,V,{},Fb);
    meshStruct.elements=E;
    meshStruct.nodes=V;
    meshStruct.Fb=Fb;
end

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;

subplot(1,2,1); hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
hp=gpatch(Fb,V,Cb,'k',faceAlpha1);
hp.Marker='.';
hp.MarkerSize=markerSize2;

colormap(gjet(6)); icolorbar;
axisGeom(gca,fontSize);

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);

drawnow;

%% Defining the boundary conditions
% The visualization of the model boundary shows colors for each side of the
% cube. These labels can be used to define boundary conditions.

%Define supported node set
bcSupportList=unique(Fb(Cb==4,:)); %Node set part of selected face

%Prescribed force nodes
bcPrescribeList=unique(Fb(Cb==3,:)); %Node set part of selected face

%%
% Visualizing boundary conditions. Markers plotted on the semi-transparent
% model denote the nodes in the various boundary condition lists.

hf=cFigure;
title('Boundary conditions','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb,V,'kw','k',0.5);

hl(1)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);
hl(2)=plotV(V(bcPrescribeList,:),'r.','MarkerSize',markerSize);

legend(hl,{'BC support','BC prescribe'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='4.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.qn_method.max_ups=max_ups;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.k=k;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type=elementType; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.VAL=bcSupportList(:)';

nodeSetName2='bcPrescribeList';
febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.VAL=bcPrescribeList(:)';

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.name='zero_displacement_xyz';
febio_spec.Boundary.bc{1}.ATTR.type='zero displacement';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.x_dof=1;
febio_spec.Boundary.bc{1}.y_dof=1;
febio_spec.Boundary.bc{1}.z_dof=1;

%Loads section
% -> Prescribed nodal forces
nodalLoadType='force';
switch nodalLoadType
    case 'load' %Apply same force to each node
        febio_spec.Loads.nodal_load{1}.ATTR.name='PrescribedForceX';
        febio_spec.Loads.nodal_load{1}.ATTR.type='nodal_load';
        febio_spec.Loads.nodal_load{1}.ATTR.node_set=nodeSetName2;
        febio_spec.Loads.nodal_load{1}.dof='x';
        febio_spec.Loads.nodal_load{1}.scale.ATTR.lc=1;
        febio_spec.Loads.nodal_load{1}.scale.VAL=appliedForce(1)/numel(bcPrescribeList);

        febio_spec.Loads.nodal_load{2}.ATTR.name='PrescribedForceY';
        febio_spec.Loads.nodal_load{2}.ATTR.type='nodal_load';
        febio_spec.Loads.nodal_load{2}.ATTR.node_set=nodeSetName2;
        febio_spec.Loads.nodal_load{2}.dof='y';
        febio_spec.Loads.nodal_load{2}.scale.ATTR.lc=1;
        febio_spec.Loads.nodal_load{2}.scale.VAL=appliedForce(2)/numel(bcPrescribeList);

        febio_spec.Loads.nodal_load{3}.ATTR.name='PrescribedForceZ';
        febio_spec.Loads.nodal_load{3}.ATTR.type='nodal_load';
        febio_spec.Loads.nodal_load{3}.ATTR.node_set=nodeSetName2;
        febio_spec.Loads.nodal_load{3}.dof='z';
        febio_spec.Loads.nodal_load{3}.scale.ATTR.lc=1;
        febio_spec.Loads.nodal_load{3}.scale.VAL=appliedForce(3)/numel(bcPrescribeList);
    case 'force' %Apply a force vector to a collection of nodes (destributed)
        febio_spec.Loads.nodal_load{1}.ATTR.name='PrescribedForceX';
        febio_spec.Loads.nodal_load{1}.ATTR.type='nodal_force';
        febio_spec.Loads.nodal_load{1}.ATTR.node_set=nodeSetName2;
        febio_spec.Loads.nodal_load{1}.value.ATTR.lc=1;
        febio_spec.Loads.nodal_load{1}.value.VAL=appliedForce/numel(bcPrescribeList);
end

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.name='LC_1';
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
%febio_spec.LoadData.load_controller{1}.extend='CONSTANT';
febio_spec.LoadData.load_controller{1}.points.pt.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode=runMode;

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    DN_magnitude=sqrt(sum(N_disp_mat(:,:,end).^2,2)); %Current displacement magnitude

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('Displacement magnitude [mm]','Interpreter','Latex')
    hp=gpatch(Fb,V_DEF(:,:,end),DN_magnitude,'k',1); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';
    gpatch(Fb,V,0.5*ones(1,3),'k',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([0 max(DN_magnitude)]);
    axis(axisLim(V_DEF)); %Set axis limits statically
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN_magnitude=sqrt(sum(N_disp_mat(:,:,qt).^2,2)); %Current displacement magnitude

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),DN_magnitude}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


function [acc, rt] = FDDL_wrapper(Y_train, label_train, Y_test , label_test, ...
                            k, lambda1, lambda2)
% function [acc, rt] = FDDL_wrapper(Y_train, label_train, Y_test , ...
%       label_test, k, lambda1, lambda2)
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 5/11/2016
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------
    if nargin == 0 % test mode
        dataset = 'myYaleB';
        N_train = 10;
        [~, Y_train, Y_test, label_train, label_test] = ...
            train_test_split(dataset, N_train);
        k = 8;
        lambda = 0.001;
        eta = 0.01;
    end
    C                = max(label_train);
    k0               = 0;
    opts.k           = k;
    opts.k0          = 0;
    opts.show_cost   = 0;
    opts.lambda1     = lambda1;
    opts.lambda2     = lambda2;
    opts.lambda3     = 0;
    opts.D_range     = k*(0:C);
    opts.D_range_ext = [opts.D_range k*C+k0];
    opts.initmode    = 'normal';
    opts.max_iter    = 100;
    opts             = initOpts(opts);
    opts.verbose      = true;
    opts.tol         = 1e-8;
    %% Train
    [D, ~, ~, ~, CoefM, ~, opts, rt] = ...
                    LRSDL(Y_train, label_train, opts);
    Y_range = label_to_range(label_train);
    C = max(label_train);
    opts.verbose = 0;
    opts.weight = 0.1;
    acc = [];
    for vgamma = [0.0001, 0.001, 0.01, 0.1]
        opts.gamma = vgamma;
        pred = FDDL_pred(Y_test, D, CoefM, opts);
        acc1 = double(numel(find(pred == label_test)))/...
            numel(label_test);
        fprintf('gamma = %.4f, acc = %.4f\n', vgamma, acc1);
        acc = [acc acc1];
    end
    acc = max(acc);
end


function [parameters, ll, ht, intercept, VCV, scores, diagnostics] = scalar_vt_vech(data,dataAsym,p,o,q,composite,startingvals,options)
% Estimation of symmetric and asymmetric scalar multivariate vech ARCH models using variance
% targeting to reduce the number of parameters needing to be estimated simultaneously
%
% USAGE:
%  [PARAMETERS,LL,HT,INTERCEPT,VCV,SCORES,DIAGNOSTICS] = scalar_vt_vech(DATA,DATAASYM,P,O,Q,COMPOSITE,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   DATAASYM     - [OPTIONAL] K by K by T array of asymmetric covariance estimators
%   P            - Positive, scalar integer representing the number of lags of the innovation process
%   O            - Non-negative scalar integer representing the number of asymmetric lags to include
%   Q            - Non-negative scalar integer representing the number of lags of conditional covariance
%   COMPOSUTE    - [OPTIONAL] String, one of:
%                    'None' - Standard K-dimensional QMLE (Default)
%                    'Diagonal' - Use only pairwise likelihoode for i,i+1, i=1,2,...,K-1
%                    'Full' - Use all pairwise likelihoods
%   STARTINGVALS - [OPTIONAL] (p+o+q) x 1 vector of starting values
%   OPTIONS      - [OPTIONAL] Options to use in the optimization (fminunc)
%
% OUTPUTS:
%   PARAMETERS   - A p+o+q column vector of parameters.  The intercept is reported in DIAGNOSTICS
%   LL           - The log likelihood at the optimum
%   HT           - A [K K T] dimension matrix of conditional covariances
%   INTERCEPT    - K by K matrix containing the intercept computed from the unconditional variance
%                    and parameters
%   VCV          - A numParams^2 square matrix of robust parameter covariances (A^(-1)*B*A^(-1)*t^(-1))
%   SCORES       - A T by numParams matrix of individual scores
%
% COMMENTS:
%    The conditional variance, H(t), of a scalar variance-targeting vech is modeled
%    as follows:
%
%      H(t) = (1-alpha(1)+...+alpha(p)-beta(1)-...-beta(q))*C + ...
%              alpha(1)*r_{t-1}'*r_{t-1} + ... + alpha(p)*r_{t-p}'*r_{t-p}+...
%              gamma(1)*n_{t-1}'*n_{t-1} + ... + gamma(o)*n_{t-p}'*n_{t-p}+...
%              beta(1)*H(t-1) +...+ beta(q)*H(t-q)
%
%    where n_{t} = r_{t} .* (r_{t}<0)
%
% EXAMPLES:
%     Estimation of a Scalar VECH(1,0,1)
%       [simulatedData, Ht, pseudoRC] = scalar_vt_vech_simulate(1000, [.02 .04 .95], .01*eye(2), 1, 1, 1, 72);
%       parameters = scalar_vt_vech(simulatedData,[],1,0,1)
%     Estimation of an asymmetric Scalar VECH(1,1,1)
%       parameters = scalar_vt_vech(simulatedData,[],1,1,1)
%     Estimation of an asymmetric Scalar VECH(1,1,1) using realized-type data
%       asymPseudoRC = zeros(size(pseudoRC));
%       for i = 1:1000
%         asymPseudoRC(:,:,i) = pseudoRC(:,:,i).*(double(simulatedData(i,:)<0)'*double(simulatedData(i,:)<0));
%       end
%       parameters = scalar_vt_vech(pseudoRC,asymPseudoRC,1,1,1);

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 10/28/2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 3
        o=0;
        q=0;
        composite = 'None';
        startingvals=[];
        options = [];
    case 4
        q=0;
        composite = 'None';
        startingvals=[];
        options = [];
    case 5
        composite = 'None';
        startingvals=[];
        options = [];
    case 6
        startingvals=[];
        options = [];
    case 7
        options = [];
    case 8
        % Nothing
    otherwise
        error('Between 3 and 8 arguments required.')
end

%data should be TxK, T>K
if ndims(data)==2
    [t,k]=size(data);
    if ~isempty(dataAsym)
        error('If DATA is a T by K matrix, DATAASYM must be empty.');
    end
    temp = zeros(k,k,t);
    dataAsym = zeros(k,k,t);
    for i=1:t
        temp(:,:,i) = data(i,:)'*data(i,:);
        dataAsym(:,:,i) = (data(i,:).*(data(i,:)<0))'*(data(i,:).*(data(i,:)<0));
    end
    data = temp;
elseif ndims(data)==3
    [k,m,t] = size(data);
    if m~=k
        error('DATA must be K by K by T is a 3D array.');
    end
    if ~isempty(dataAsym)
        if ndims(dataAsym)~=3
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
        [k2,m2,t2]=size(dataAsym);
        if any([k m t]~=[k2 m2 t2])
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
    end
end

if min(t,k)<2 || t<k
    error('DATA must be a T by K matrix or a K by K by T 3D array, T>K>1');
end

%p, o, q much be non-negative scalars
if length(p)>1 || any(p<1) || floor(p)~=p
    error('P must be a positive scalar');
end
if isempty(o)
    o = 0;
end
if length(o)>1 || any(o<0) || floor(o)~=o
    error('O must be a non-negative scalar');
end
if o>0 && isempty(dataAsym)
    error('DATAASYM must be non-empty if O>0.')
end

if isempty(q)
    q = 0;
end
if length(q)>1 || any(q<0) || floor(q)~=q
    error('Q must be a non-negative scalar');
end

if isempty(composite)
    composite = 'None';
end
switch lower(composite)
    case {'none'}
        useComposite = 0;
    case {'diagonal'}
        useComposite = 1;
    case {'full'}
        useComposite = 2;
end

% Startingvals must have p+o+q parameters sum(alpha)+kappa*sum(gamma)+sum(beta)<1
C = mean(data,3);
if o>0
    Casym = mean(dataAsym,3);
    kappa = 1 / (max(eig(C^(-0.5)*Casym*C^(-0.5))) + eps);
else
    Casym = zeros(k);
    kappa = 2;
end
if  ~isempty(startingvals)
    if size(startingvals,2)>size(startingvals,1)
        startingvals = startingvals';
    end
    if length(startingvals)<(p+o+q)
        error('STARTINGVALS should be a P+O+Q by 1 vector');
    end
    %Only validate if provided
    A=startingvals(1:p);
    G=startingvals(p+1:p+o);
    B=startingvals(p+o+1:p+o+q);
    if (sum(A)+sum(G)/kappa+sum(B))>=.999998
        error('Weighted sum of STATINGVALUES must be less than 1. See Comments.');
    end
    if any(A<0) || any(B<0) || any(G<0)
        error('STARTINGVALS must all be nonnegative.');
    end
end

%Make sure options is a valid option structure
if isempty(options)
    options=optimset('fminunc');
    options.Display='iter';
    options.Diagnostics='on';
    options.LargeScale='off';
end
try
    optimset(options);
catch ME
    error('OPTIONS is not a valid options structure');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Compute the backCast
backCast = zeros(k);
backCastAsym = zeros(k);
tau = max(ceil(sqrt(t)),k);
weights = .06 * .94.^(0:tau);
weights = weights / sum(weights);
for i=1:tau
    backCast = backCast + weights(i) * data(:,:,i);
    if o>0
        backCastAsym = backCastAsym + weights(i) * dataAsym(:,:,i);
    end
end

%Augment the data with backcasts
[startingvals,lls,output_parameters] = scalar_vt_vech_starting_values(startingvals,data,dataAsym,p,o,q,C,Casym,kappa,useComposite,backCast,backCastAsym); %#ok<NASGU>

% finally to transform the parameters to the unrestricted equivalents
startingvals=scalar_vt_vech_transform(startingvals,p,o,q,kappa);

[parameters,ll,exitflag,output]=fminunc('scalar_vt_vech_likelihood',startingvals,options,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,true);
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if exitflag<=0 &&  ll<lls(1)
    %Did not converge, but function improved
    options.MaxFunEvals=4*100*(p+q);
    options.MaxIter=2*100*(p+q);
    parameters=fminunc('scalar_vt_vech_likelihood',parameters,options,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,true);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%

parameters=scalar_vt_vech_itransform(parameters,p,o,q,kappa);
if nargout>1
    [ll,~,ht]=scalar_vt_vech_likelihood(parameters,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,false);
    ll=-ll;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>4
    k2 = k*(k+1)/2;
    CVech=vech(C);
    if o==0
        momentCount = k2;
        CasymVech = [];
    else
        momentCount = 2*k2;
        CasymVech=vech(Casym);
    end

    A = zeros(momentCount + p + o + q);
    A(1:momentCount, 1:momentCount) = - t * eye(momentCount);
    jointParameters=[CVech;CasymVech;parameters];

    % A will be k2 + p + q square
    scores = zeros(t,momentCount+p+q);
    scoreCount = 1;
    for i=1:k
        for j=i:k
            scores(:,scoreCount) = squeeze(data(j,i,:));
            if o>0
                scores(:,k2+scoreCount) = squeeze(dataAsym(j,i,:));
            end
            scoreCount = scoreCount  + 1;
        end
    end
    [~,gt] = gradient_2sided(@scalar_vt_vech_likelihood,parameters,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,false,useComposite,false);
    scores(:,momentCount+1:momentCount+p+o+q) = gt;
    A(momentCount+1:momentCount+p+o+q,:) = hessian_2sided_nrows(@scalar_vt_vech_likelihood,jointParameters,p+o+q,data,dataAsym,p,o,q,C,Casym,kappa,backCast,backCastAsym,true,useComposite,false);

    A = A/t;
    B = covnw(scores);
    Ainv = inv(A);
    VCV = Ainv*B*Ainv'/t; %#ok<MINV>
    VCV = VCV(momentCount+1:momentCount+p+o+q,momentCount+1:momentCount+p+o+q);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
diagnostics.EXITFLAG=exitflag;
diagnostics.ITERATIONS=output.iterations;
diagnostics.FUNCCOUNT=output.funcCount;
diagnostics.MESSAGE=output.message;
diagnostics.kappa=kappa;
alpha = sum(parameters(1:p));
gamma = sum(parameters(p+1:p+o));
beta = sum(parameters(p+o+1:p+o+q));
diagnostics.intercept = C*(1-alpha-beta);
diagnostics.composite = composite;
if o>0
    diagnostics.intercept  = diagnostics.intercept  - gamma*Casym;
end
intercept = diagnostics.intercept;


function exact = p05_exact ( )

%*****************************************************************************80
%
%% P05_EXACT returns the exact integral for problem 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  exact = 4.0 / 9.0;

  return
end


function H=transferfunction(g,L)
%TRANSFERFUNCTION  The transferfunction of a filter
%   Usage:  H=transferfunction(g,L);
%
%   `transferfunction(g,L)` computes the transferfunction of length *L*
%   of the filter defined by *g*.
%
%   See also: pfilt

complainif_argnonotinrange(nargin,2,2,mfilename);

[g,info] = comp_fourierwindow(g,L,upper(mfilename));

H=comp_transferfunction(g,L);


function roi = roiGrayDisk(seg, mr, radius, startPt);
% Create an ROI as a disk of gray matter with the specified startPt
% and radius.
%
% roi = roiGrayDisk(seg, mr, [radius=dialog], [startPt=mesh cursor]);
%
%
% ras, 02/2007.
if nargin<2, error('Not enough input args.');	end
if notDefined('mapMethod'),     mapMethod = 'all'; end

msh = segGet(seg, 'SelectedMesh');
[nodes edges] = segGet(seg, 'gray');

if isempty(msh.vertexGrayMap) | all(msh.vertexGrayMap(:)==0)
    msh.vertexGrayMap = mrmMapVerticesToGray(msh.initVertices, nodes, ...
							msh.mmPerVox, edges);
end

if notDefined('startPt')
	vtx = mrmGet(msh, 'cursorvertex');

	if vtx<1
		error('Mesh Cursor not pointing to a vertex on the mesh.')
	end

	I = msh.vertexGrayMap(1,vtx); % layer 1 node for this vertex
	startPt = nodes([2 1 3],I);
end

if notDefined('radius')
	r = inputdlg({'Enter Gray Disk Radius (mm):'}, mfilename, 1, {'3'});
	radius = r{1};
end

%% initialize empty ROI based on the same anatomy coords as the segmentation
roi = roiCreate('I|P|R');
roi.voxelSize = msh.mmPerVox;
if isstruct(mr)
	roi.referenceMR = mr.name;
else
	roi.referenceMR = mr;
end

%% find index of gray node closest to start point
startNode = segGet(seg, 'NearestNode', startPt);

%% compute the distance between each gray node and the start point
dist = mrManDist(nodes, edges, startNode, msh.mmPerVox, -1, radius);

%% find those nodes whose distance is within the radius
inside = find(dist >= 0); % only searches within radius; invalid is -1

%% get the coords of those nodes, set as ROI coords
roi.definedCoords = nodes([2 1 3],inside);
roi.coords = roi.definedCoords;
roi = roiCheckCoords(roi, mr);
roi.name = sprintf('Disk ROI (%i mm about %s)', radius, num2str(startPt'));
roi.comments = [sprintf('Created by %s %s \n', mfilename, datestr(now)) ...
				sprintf('Start Point %s \n', num2str(startPt)) ...
				sprintf('Start Node %i \n', startNode) ...
				sprintf('Radius %i mm', radius)];

return


function top = startofPOWCone(K)

top = 1 + K.f + K.l + sum(K.q) + 3*K.e;


function [cl,cd] = bpolar(BladeState,AC)
% To make your own bpolar:
% BladeState is the state of the blade element with matrix components:
% BladeState =
%            M: Mach number
%        alpha: angle of attack (radians)
%            r: radius location (nondimensional)
%          psi: azimuth location (radians)
%     alphadot: rate of change of angle of attack (rad/s)
%           Re: Reynolds number
% all components on BladeState inputs are m x n, where n is the number of
% blade elements, and m is the uniformly distributed number of azimuth
% elements.
%
% NB: this function should run VERY fast, so avoid loops and extensive
% interpolation.
% see also lininterp1f
%
% AC is a structure with AC data (see documentation)
%
% cl and cd are the m x n arrays containing each blade element's lift and
% drag coefficients.
% note that output coefficients will be non-dimensionalized by a constant
% blade chord, typically the average blade chord.
%
% The blade polar function is responsible for capturing all important rotor
% phenomena, including reverse flow, retreating blade stall, and advancing
% tip critical mach number.
%
% to capture reverse flow effects, the drag polar should be able to handle
% alphas +/- 360 deg
%
% critical mach drag rise and stall should also be modeled.

% This file is a simple but reasonable drag polar
%alphadot is not used for this simple polar, but can be used for more
%complex historesis stall behavior, etc.

RootCutout = .15;
Clmax = 1.35;

%%
alpha = BladeState.alpha;
r = BladeState.r;
M = BladeState.M;

tau = 2*pi;

astallmax = Clmax/tau;

% put all values between 0 and 360
ind = alpha<0;
while any(ind(:))
    alpha(ind) = alpha(ind)+tau;
    ind = alpha<0;
end
ind = alpha>tau;
while any(ind(:))
    alpha(ind) = alpha(ind)-tau;
    ind = alpha>tau;
end

% flat plate:
% fpClmax = 1.05;
% fpCl = @(a) fpClmax*sin(2*a);
% fpCd = @(x) -.9*cos(2*x) +.92;
cl = 1.05*sin(2*alpha);
cd= -.9*cos(2*alpha)+.92;

ind = alpha<=astallmax;
cl(ind) = tau*alpha(ind);
cd(ind)=.006+.1737*alpha(ind).^2;

ind = alpha>=tau-astallmax;
cl(ind) = tau*(alpha(ind)-tau);
cd(ind)=.006+.1737*(alpha(ind)-tau).^2;

% Critical Mach
absalpha = abs(alpha);
absalpha(alpha>pi/2) = abs(alpha(alpha>pi/2)-pi);
absalpha(alpha>3*pi/2) = abs(alpha(alpha>3*pi/2)-tau);
Mcrit = max(.55,-.02*absalpha+.765);
% from ESDU AERO W.00.03.01

dM = abs(M)-Mcrit;
drise = 801.6*exp(-((dM-1.441)/.4146).^2);
drise(dM>.3147) = .5;
cd = cd + drise;

%tip loss
ind = r>.97;
cl(ind)=cl(ind).*(1-(r(ind)-.97)/.03);

%% root cutout
cl(r<RootCutout) = 0;
cd(r<RootCutout) = .1;


classdef OptimizerDualNestedInPrimal < Optimizer_PrimalDual

    properties (GetAccess = public, SetAccess = protected)
        type = 'DualNestedInPrimal'
    end

    properties (Access = private)
        desVarChangedValue
        costIncrease
        constraintProjector
    end

    methods (Access = public)

        function obj = OptimizerDualNestedInPrimal(cParams)
            obj.initOptimizer(cParams);
            obj.createLagrangian();
            obj.createOptimizerUnconstrained(cParams.uncOptimizerSettings)
            obj.createConstraintProjector();
        end

        function solveProblem(obj)
            obj.hasFinished = false;

            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();

            obj.updateOldValues();
            obj.unconstrainedOptimizer.startLineSearch();
            obj.unconstrainedOptimizer.updateConvergenceParams();
            obj.refreshMonitoring();
            obj.printOptimizerVariable();
       %     obj.printHistory();
            obj.nIter = obj.nIter+1;

            obj.designVariable.updateOld();
            obj.computeFeasibleDesignVariable();

            obj.cost.computeFunctionAndGradient();

            obj.updateOldValues();

            obj.unconstrainedOptimizer.updateConvergenceParams();
            obj.unconstrainedOptimizer.updateLineSearch();
            obj.refreshMonitoring();
            obj.printHistory();
            obj.saveDesignVariable();
            obj.printOptimizerVariable();
       %     obj.printHistory();
            obj.nIter = obj.nIter+1;

            %obj.hasFinished = false;
            obj.updateStatus();

            while ~obj.hasFinished

                obj.unconstrainedOptimizer.tryLineSearch();
                while ~obj.hasUnconstraintedOptimizerConverged()
                    obj.restartValues();
                    obj.computeValue();
                    if ~obj.hasUnconstraintedOptimizerConverged()
                        obj.unconstrainedOptimizer.updateLineSearch();
                    end
                end

                obj.cost.computeFunctionAndGradient();

                obj.unconstrainedOptimizer.updateConvergenceParams();

                obj.updateConvergenceStatus();
                obj.updateStatus();

                obj.updateOldValues();

                obj.refreshMonitoring();
                obj.printOptimizerVariable();
                obj.printHistory();
                obj.saveDesignVariable();
                obj.nIter = obj.nIter+1;
            end
            %obj.printOptimizerVariable();
            %obj.printHistory();
            obj.hasConverged = 0;
            obj.printHistoryFinalValues();
        end

        function saveDesignVariable(obj)
            x = obj.designVariable.value;
            mesh = obj.designVariable.mesh.innerMeshOLD;
            path = 'Output/CantileverTetraPerimeterTotal/DesignVariable';
            %save([path,num2str(obj.nIter)],'x','mesh');
        end

        function restartValues(obj)
            obj.designVariable.restart();
            obj.dualVariable.restart();
            obj.cost.restart();
            obj.constraint.restart();
            obj.updateLagrangian();
        end

        function computeFeasibleDesignVariable(obj)
            if obj.isNotFeasible()
                obj.constraint.updateOld();
                obj.updateLagrangian();
                obj.lagrangian.updateOld();

                obj.unconstrainedOptimizer.tryLineSearch();
                %kappa = obj.unconstrainedOptimizer.lineSearch.kappa;
                %obj.unconstrainedOptimizer.lineSearch.kappa = kappa;

                obj.constraintProjector.project();
            end
        end

        function itIsNot = isNotFeasible(obj)
            itIsNot = ~obj.isFeasible();
        end

        function update(obj)
            while ~obj.hasUnconstraintedOptimizerConverged()
                obj.computeValue();
                obj.unconstrainedOptimizer.updateLineSearch();
            end
        end

    end

    methods (Access = protected)

        function createLagrangianSettings(obj)
            cParams.type         = 'Lagrangian';
            cParams.cost         = obj.cost;
            cParams.constraint   = obj.constraint;
            cParams.dualVariable = obj.dualVariable;
            obj.lagrangianSettings = cParams;
        end

        function updateLagrangian(obj)
            obj.lagrangian.computeFunction();
            obj.lagrangian.computeGradient();
        end

        function updateOldValues(obj)
            obj.designVariable.updateOld();
            obj.dualVariable.updateOld();
            obj.cost.updateOld();
            obj.constraint.updateOld();
            obj.updateLagrangian();
            obj.lagrangian.updateOld();
        end

    end

    methods (Access = private)

        function createConstraintProjector(obj)
            cParams.cost           = obj.cost;
            cParams.constraint     = obj.constraint;
            cParams.designVariable = obj.designVariable;
            cParams.dualVariable   = obj.dualVariable;
            cParams.lagrangian     = obj.lagrangian;
            cParams.targetParameters = obj.targetParameters;
            cParams.unconstrainedOptimizer = obj.unconstrainedOptimizer;
            obj.constraintProjector = ConstraintProjector(cParams);
        end

        function computeValue(obj)
            obj.constraintProjector.project();
            obj.cost.computeFunction();
            obj.updateLagrangian();
        end

        function itHas = hasUnconstraintedOptimizerConverged(obj)
            itHas = obj.isStepAcceptable() || obj.isLineSeachTooSmall();
        end

        function itIs = isStepAcceptable(obj)
            incr = obj.lagrangian.computeIncrement();
            costHasDecreased = incr < 0;
            itIs = costHasDecreased;
        end

        function itIs = isLineSeachTooSmall(obj)
            itIs = obj.unconstrainedOptimizer.isLineSearchTooSmall();
        end

    end
end


function kernel = calibrate4D(obj, AtA, nCha, coil)
% retrieve calibration kernel from 4D calibration region
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

sampling = ones([obj.kernelSize,nCha],obj.measPara.precision);

dummyK = zeros([obj.kernelSize,nCha],obj.measPara.precision);
dummyK((end+1)/2,(end+1)/2,(end+1)/2,coil) = 1;
idxY = find(dummyK);
sampling(idxY) = 0;
idxA = find(sampling);

Aty = AtA(:,idxY); Aty = Aty(idxA); % correlation values to target point, take complete neighbourhood and not just aquired ones
AtA = AtA(idxA,:); AtA =  AtA(:,idxA); % kick out the searched point

kernel = sampling*0;

lambda = norm(AtA,'fro')/size(AtA,1)*obj.calibTyk;

cnd = cond(AtA + eye(size(AtA))*lambda);
if(cnd == inf)
    rawkernel = pinv(AtA + eye(size(AtA))*lambda)*Aty; % grappa weighting values
else
    rawkernel = (AtA + eye(size(AtA))*lambda)\Aty; % grappa weighting values
end
kernel(idxA) = rawkernel;

end


classdef PTKDensityInterpolation < PTKPlugin
    % PTKDensityInterpolation. Plugin for interpolating density values to a
    % different voxel size
    %
    %     This is a plugin for the Pulmonary Toolkit. Plugins can be run using
    %     the gui, or through the interfaces provided by the Pulmonary Toolkit.
    %     See PTKPlugin.m for more information on how to run plugins.
    %
    %     Plugins should not be run directly from your code.
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties
        ButtonText = 'Density Interpolation'
        ToolTip = 'Recomputes image density with different sized voxels'
        Category = 'Lungs'

        AllowResultsToBeCached = true
        AlwaysRunPlugin = false
        PluginType = 'ReplaceOverlay'
        HidePluginInDisplay = false
        FlattenPreviewImage = false
        PTKVersion = '1'
        ButtonWidth = 6
        ButtonHeight = 1
        GeneratePreview = false
        Visibility = 'Developer'
    end

    methods (Static)
        function results = RunPlugin(application, reporting)

            % The size of the 'voxels' for calculating the density.
            % Note if you want to change this, you should change the above
            % property
            %     AlwaysRunPlugin = false
            % so that the plug is forced to re-run

            interp_voxel_size_mm = [5, 5, 5];

            % Fetch the intensity of just the lung regions
            roi = application.GetResult('PTKLungROI');
            left_and_right_lungs = application.GetResult('PTKLeftAndRightLungs');

            reporting.ShowProgress('Finding lung region for density');
            mask = left_and_right_lungs.RawImage > 0;
            roi_lung = int16(roi.RawImage).*int16(mask);

            interp_voxel_size_units = interp_voxel_size_mm./roi.VoxelSize;
            image_size = roi.ImageSize;
            i_span = 1 : interp_voxel_size_units(1) : image_size(1);
            j_span = 1 : interp_voxel_size_units(2) : image_size(2);
            k_span = 1 : interp_voxel_size_units(3) : image_size(3);
            [interp_i, interp_j, interp_k] = ndgrid(i_span, j_span, k_span);

            % Interpolate dentisty values
            reporting.ShowProgress('Interpolating to new voxel grid');
            roi_interp = interpn(single(roi_lung), interp_i, interp_j, interp_k, '*linear');

            % Interpolate mask (gives a measure of how much of each voxel is
            % within the lung vs outside the lung)
            reporting.ShowProgress('Interpolating to new voxel size');
            roi_mask = interpn(single(mask), interp_i, interp_j, interp_k, '*linear');

            % Rescale voxels that are partially outside the lung, and remove
            % those more than 50% outside of the lung
            roi_interp = roi_interp./roi_mask;
            roi_interp(roi_mask < 0.5) = 0;

            roi_interp = roi_interp/max(roi_interp(:));

            reporting.ShowProgress('Finding coordinates on original grid');
            i_span_r = single(1 + (1:image_size(1))/interp_voxel_size_units(1));
            j_span_r = single(1 + (1:image_size(2))/interp_voxel_size_units(2));
            k_span_r = single(1 + (1:image_size(3))/interp_voxel_size_units(3));

            [interp_i, interp_j, interp_k] = ndgrid(i_span_r, j_span_r, k_span_r);
            interp_i = min(size(roi_interp, 1), round(interp_i));
            interp_j = min(size(roi_interp, 2), round(interp_j));
            interp_k = min(size(roi_interp, 3), round(interp_k));

            reporting.ShowProgress('Interpolation to original grid');
            indices = sub2ind(size(roi_interp), (interp_i(:)), (interp_j(:)), (interp_k(:)));
            results_raw = zeros(image_size, 'single');
            results_raw(:) = roi_interp(indices);

            results = roi.BlankCopy;

            results.ChangeRawImage(single(results_raw));
            results.ImageType = PTKImageType.Scaled;

        end
    end
end


function [ point_num2, edge_num2, face_num2, face_order_max2 ] = ...
  dual_size_3d ( point_num, edge_num, face_num, face_order_max, ...
  point_coord, face_order, face_point )

%*****************************************************************************80
%
%% DUAL_SIZE_3D determines sizes for a dual of a shape in 3D.
%
%  Discussion:
%
%    We don't actually need FACE_POINT as input here.  But since the
%    three arrays occur together everywhere else, it seems unnecessarily
%    user-confusing to vary the usage here!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Input, integer FACE_NUM, the number of faces.
%
%    Input, integer FACE_ORDER_MAX, the maximum number of vertices per face.
%
%    Input, real POINT_COORD(3,POINT_NUM); POINT_COORD(*,J) is
%    the X, Y and Z coordinates of point J.
%
%    Input, integer FACE_ORDER(FACE_NUM), the number of vertices per face.
%
%    Input, integer FACE_POINT(FACE_ORDER_MAX,FACE_NUM); FACE_POINT(I,J)
%    is the index of the I-th point in the J-th face.  The
%    points are listed in the counter-clockwise direction defined
%    by the outward normal at the face.
%
%    Output, integer POINT_NUM2, the number of points in the dual.
%
%    Output, integer EDGE_NUM2, the number of edges in the dual.
%
%    Output, integer FACE_NUM2, the number of faces in the dual.
%
%    Output, integer FACE_ORDER_MAX2, the maximum number of vertices per face
%    in the dual.
%

%
%  These values are easy to compute:
%
  point_num2 = face_num;
  edge_num2 = edge_num;
  face_num2 = point_num;
%
%  To determine FACE_ORDER_MAX2 is not so easy.
%  You have to construct the FACE_ORDER array for the dual shape.
%  The order of a dual face is the number of edges that the vertex occurs in.
%  But then all we have to do is count how many times each item shows up
%  in the FACE_POINT array.
%
  face_order_max2 = 0;
  face_order2(1:face_num2) = 0;

  for face = 1 : face_num
    for i = 1 : face_order(face)
      face2 = face_point(i,face);
      face_order2(face2) = face_order2(face2) + 1;
    end
  end

  face_order_max2 = max ( face_order2(1:face_num2) );

  return
end


% DEMSWISSROLLFULLLLE1 Demonstrate LLE on the oil data.

% MLTOOLS

[Y, lbls] = lvmLoadData('swissRollFull');

options = lleOptions(4, 2);
model = lleCreate(2, size(Y, 2), Y, options);
model = lleOptimise(model);

lvmScatterPlotColor(model, model.Y(:, 2));

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, model.Y(:, 2), 'SwissRollFull', 1, true);
end


function[varargout]=col2mat(varargin)
%COL2MAT  Expands 'column-appended' data into a matrix.
%
%   M=COL2MAT(C), where C is a column vector of data segments separated by
%   NANs, returns a matrix M in which each segment has its own column.
%
%   M will have enough rows to fit the longest data segment. Segments are
%   read into columns of M from the top down, leaving empty spaces at the
%   bottom which are filled in with NANs.
%
%   If the data is complex, then gaps are filled with NAN+SQRT(-1)*NAN;
%
%   [M1,M2,...,MN]=COL2MAT(C1,C2,...,CN), where the CN are input column
%   vectors, also works, as does [M1,M2,...,MN]=COL2MAT(MAT), where MAT is
%   a matrix of column vectors.  In both cases the locations of NaNs in the
%   first column is used as the reference for the others.
%
%   COL2MAT(C1,C2,...); with no output arguments overwrites the original
%   input variables.
%
%   COL2MAT, MAT2COL, and COLBREAKS together form a system for moving data
%   with segments of nonuniform length rapidly back and forth between a
%   column format and a padded-matrix format.
%
%   Note that while COL2CELL and CELL2COL work for arrays having multiple
%   columns, COL2MAT only works with column vectors.
%
%   See also MAT2COL, COLBREAKS, COL2CELL, CELL2COL.
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2016 J.M. Lilly --- type 'help jlab_license' for details

%no loops!

if ischar(varargin{1})
    if strcmpi(varargin{1}(1:3),'--t')
        return
    end
end

if isempty(varargin{1})
   for i=1:nargout
       varargout{i}=[];
   end
   return
end

if nargin>1
	for i=1:nargin
        	eval(['data(:,' int2str(i) ')=varargin{' int2str(i) '};'])
	end
else
	data=varargin{1};
end

for ii=1:size(data,2)
	col=data(:,ii);
	if ii==1;
        %Account for potential missing NaNs at the end
        if ~isnan(col(end))
            col(end+1)=nan;
            data(end+1,:)=nan;
        end
		%use index into NANs and index derivative
		%to determine size of new matix
		nani=find(isnan(col));
		dnani=diff([0;nani]);
		nrows=max(dnani);
		ncols=length(nani);

		%determine index into top of each column ==a
		%and index into first NAN in each column ==b
		a=1+nrows*(0:ncols-1)';
        %size(dnani)
        %size(a)
		b=dnani+a;
		index=zeros(nrows*ncols,1);

		b=b(b<length(index));
		%mark all the numbers between each a and each b
		index(a)=1;
		index(b)=index(b)-1;%this matters (a may equal b)
		index=find(cumsum(index));
		if length(index)>length(col)
		   index=index(1:length(col));
		end
	end
	mat=nan*ones(nrows,ncols);
    mat(index)=col;
    if ~isreal(mat)
        vswap(mat,nan,nan+sqrt(-1)*nan);
    end
	varargout{ii}=mat;
end

if nargout>nargin
	varargout{int2str(size(data,2)+1)}=index;
end

if nargout==0
  eval(to_overwrite(nargin));
end


function [p] = eeg_peaks(p)

% eeg_peaks - Find peaks in EEG data
%
% Usage: [p] = eeg_peaks(p)
%
% The p structure is explained in eeg_toolbox_defaults.
% For this function, it must contain the field called
% 'volt.data,' which is assumed to be a matrix
% with N rows of EEG sample values from M columns
% of electrodes.
%
% The indices to peak values are returned to
% p.volt.peaks.data (which is size(p.volt.data)).
% The values are:
%                    0   no peak
%                    1   a +ve peak
%                   -1   a -ve peak
%
% This function finds all peaks in a waveform, where
% f(x) changes from an increasing to a decreasing
% function and vice versa). These are local minima
% and maxima that have a first derivate of zero and
% satisfy the first derivative test.
%
% Thus, the +ve/-ve peak refers to peaks and troughs
% in a waveform, regardless of the +ve/-ve value of
% the waveform at that point. The peak values are
% commonly +ve/-ve potentials, but they may not be.
% A +ve peak can have a -ve value where the change
% from increasing f(x) to decreasing f(x) occurs
% entirely in the range of negative values.
%
% All the +ve/-ve peak values are returned in:
%
%   p.volt.peaks.all
%   p.volt.peaks.pos
%   p.volt.peaks.neg
%
% If the p.volt.timeArray is defined, then the timing
% of the peaks is returned in:
%
%   p.volt.peaks.alltimes
%   p.volt.peaks.postimes
%   p.volt.peaks.negtimes
%
% eg,   p.volt.timeArray = linspace(-20,20,200)';
%       p.volt.data = sin(linspace(-20,20,200))';
%       p.volt.data(:,2) = sin(-1 .* linspace(-20,20,200))';
%      [p] = eeg_peaks(p);
%       scatter(p.volt.timeArray(p.volt.peaks.all),p.volt.data(p.volt.peaks.all))
%       hold on
%       plot(p.volt.timeArray,p.volt.data)
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:52 $

% Licence:  www.gnu.org GPL, no implied or express warranties
% Created:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%                    - adapted a perl foreach loop to peaks_matrix
%                      to take advantage of matlab speed.
%           05/2002, Darren.Weber_at_radiology.ucsf.edu
%                    - used matlab 'diff' command
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('EEG_PEAKS\n...calculating peaks...'); tic;

p.volt.peaks.data = peaks_matrix(p.volt.data);

% Find all the peak values
p.volt.peaks.all = p.volt.data .* (p.volt.peaks.data ~= 0);
p.volt.peaks.pos = p.volt.data .* (p.volt.peaks.data  > 0);
p.volt.peaks.neg = p.volt.data .* (p.volt.peaks.data  < 0);

if ~isfield(p.volt,'timeArray'),
    fprintf('\n...p doesn''t contain ''volt.timeArray''.\n');
    return
elseif isempty(p.volt.timeArray),
    fprintf('\n...p.volt.timeArray is empty.\n');
    return
end

% make sure that volt.timeArray is same size as volt.data
if ~isequal(size(p.volt.timeArray),size(p.volt.data)),
    p.volt.timeArray = repmat(p.volt.timeArray(:,1),1,size(p.volt.data,2));
end
% Find the peak timing
p.volt.peaks.alltimes = p.volt.timeArray .* (p.volt.peaks.data ~= 0);
p.volt.peaks.postimes = p.volt.timeArray .* (p.volt.peaks.data  > 0);
p.volt.peaks.negtimes = p.volt.timeArray .* (p.volt.peaks.data  < 0);

t = toc;
fprintf('done (%5.2f sec)\n',t);
return

function [peaks] = peaks_matrix(data)

    % This function should be quicker than that below.
    % The code below remains as it has been proved to
    % be reliable, but this code effectively replaces it.
    % Both code sets produce the same results.

    [r,c] = size(data);
    % replicate data, but staggered one point
    data2 = zeros(1,c);
    data2(2:r+1,:) = data;
    % add zero values to end of data points
    data(r+1,:) = zeros(1,c);
    % generate boolean difference between two matrices
    dif = data < data2;

    % Now replicate this boolean matrix
    [br,bc] = size(dif);
    dif2 = zeros(1,bc);
    dif2(2:br+1,:) = dif;
    % add zeros to end of boolean dif
    dif(br+1,:) = 0;

    % Calculate the peaks
    peaks = dif(2:r+1,:) - dif2(2:r+1,:);

    % now trim peaks dif back to size of data
    peaks(1,:) = 0; % first point cannot be a peak
    peaks(r,:) = 0; % last point cannot be a peak

    % Could use matlab diff command, like this
    %[r,c] = size(data);
    %decdat = (diff(data)<=0) .* -1;
    %incdat =  diff(data)>=0;

    %zrow = zeros(1,c);
    %dirdat1 = [ zrow; decdat + incdat ];
    %dirdat2 = [ decdat + incdat; zrow ];

    %negpeaks = ((dirdat1 - dirdat2) < 0) .* -1;
    %pospeaks =  (dirdat1 - dirdat2) > 0;
    %peaks = negpeaks + pospeaks;

    %peaks(1,:)   = 0; % first point cannot be a peak
    %peaks(end,:) = 0; % last point cannot be a peak

return

% The function 'peaks_matrix' above replaced the
% following for loop (02/2002, Darren Weber)

% foreach electrode
%for e = 1:size(p.volt.data,2),
%	v = p.volt.data(:,e);
%    n = 1;
%    volt = v(n);
%	while (n <= size(v,1)),
%        if(volt < v(n)),
%            volt = v(n); n = n + 1;
%            if(volt < v(n)), p.volt.peaks.data(n-1,e) = 0;
%            else             p.volt.peaks.data(n-1,e) = 1;
%            end
%        elseif(volt > v(n)),
%            volt = v(n); n = n + 1;
%            if(volt > v(n)), p.volt.peaks.data(n-1,e) = 0;
%            else             p.volt.peaks.data(n-1,e) = -1;
%            end
%        else
%            volt = v(n); n = n + 1;
%           p.volt.peaks.data(n-1,e) = 0;
%        end
%        if isequal(n,size(v,1)),
%            break;
%        end
%    end
%    p.volt.peaks.data(1,e) = 0; % first point cannot be a peak
%    p.volt.peaks.data(n,e) = 0; % last point cannot be a peak
%end
%p.volt.peaks.all = find(p.volt.peaks.data ~= 0);
%p.volt.peaks.pos = find(p.volt.peaks.data  > 0);
%p.volt.peaks.neg = find(p.volt.peaks.data  < 0);
%return


function g = glmderiv(net, x)
%GLMDERIV Evaluate derivatives of GLM outputs with respect to weights.
%
%	Description
%	G = GLMDERIV(NET, X) takes a network data structure NET and a matrix
%	of input vectors X and returns a three-index matrix mat{g} whose  I,
%	J, K element contains the derivative of network output K with respect
%	to weight or bias parameter J for input pattern I. The ordering of
%	the weight and bias parameters is defined by GLMUNPAK.
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
errstring = consist(net, 'glm', x);
if ~isempty(errstring)
    error(errstring);
end

ndata = size(x, 1);
if isfield(net, 'mask')
  nwts = size(find(net.mask), 1);
  mask_array = logical(net.mask)*ones(1, net.nout);
else
  nwts = net.nwts;
end
g = zeros(ndata, nwts, net.nout);

temp = zeros(net.nwts, net.nout);
for n = 1:ndata
    % Weight matrix w1
    temp(1:(net.nin*net.nout), :) = kron(eye(net.nout), (x(n, :))');
    % Bias term b1
    temp(net.nin*net.nout+1:end, :) = eye(net.nout);
    if isfield(net, 'mask')
	g(n, :, :) = reshape(temp(find(mask_array)), nwts, net.nout);
    else
	g(n, :, :) = temp;
    end
end


function logmmse(filename,outfile)

%
%  Implements the logMMSE algorithm [1].
%
%  Usage:  logmmse(noisyFile, outputFile)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%
%
%  Example call:  logmmse('sp04_babble_sn10.wav','out_log.wav');
%
%  References:
%   [1] Ephraim, Y. and Malah, D. (1985). Speech enhancement using a minimum
%       mean-square error log-spectral amplitude estimator. IEEE Trans. Acoust.,
%       Speech, Signal Process., ASSP-23(2), 443-445.
%
% Authors: Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<2
   fprintf('Usage: logmmse(noisyfile.wav,outFile.wav) \n\n');
   return;
end

[x, Srate, bits]= wavread( filename);	%nsdata is a column vector

% =============== Initialize variables ===============

len=floor(20*Srate/1000); % Frame size in samples
if rem(len,2)==1, len=len+1; end;
PERC=50; % window overlap in percent of frame size
len1=floor(len*PERC/100);
len2=len-len1; % update rate in samples

win=hanning(len);  % define window
win = win*len2/sum(win);  % normalize window for equal level output

% Noise magnitude calculations - assuming that the first 6 frames is
% noise/silence

nFFT=2*len;
noise_mean=zeros(nFFT,1);
j=1;
for m=1:6
    noise_mean=noise_mean+abs(fft(win.*x(j:j+len-1),nFFT));
    j=j+len;
end
noise_mu=noise_mean/6;
noise_mu2=noise_mu.^2;

%--- allocate memory and initialize various variables

x_old=zeros(len1,1);
Nframes=floor(length(x)/len2)-floor(len/len2);
xfinal=zeros(Nframes*len2,1);

%===============================  Start Processing =======================================================
%
k=1;
aa=0.98;
mu=0.98;
eta=0.15;

ksi_min=10^(-25/10);

for n=1:Nframes

    insign=win.*x(k:k+len-1);

    spec=fft(insign,nFFT);
    sig=abs(spec); % compute the magnitude
    sig2=sig.^2;

    gammak=min(sig2./noise_mu2,40);  % limit post SNR to avoid overflows
    if n==1
        ksi=aa+(1-aa)*max(gammak-1,0);
    else
        ksi=aa*Xk_prev./noise_mu2 + (1-aa)*max(gammak-1,0);     % a priori SNR
        ksi=max(ksi_min,ksi);  % limit ksi to -25 dB
    end

    log_sigma_k= gammak.* ksi./ (1+ ksi)- log(1+ ksi);
    vad_decision= sum(log_sigma_k)/ len;
    if (vad_decision< eta)
        % noise only frame found
        noise_mu2= mu* noise_mu2+ (1- mu)* sig2;
    end
    % ===end of vad===

    A=ksi./(1+ksi);  % Log-MMSE estimator
    vk=A.*gammak;
    ei_vk=0.5*expint(vk);
    hw=A.*exp(ei_vk);

    sig=sig.*hw;
    Xk_prev=sig.^2;

    xi_w= ifft( hw .* spec,nFFT);
    xi_w= real( xi_w);

    xfinal(k:k+ len2-1)= x_old+ xi_w(1:len1);
    x_old= xi_w(len1+ 1: len);

    k=k+len2;

end

wavwrite(xfinal,Srate,16,outfile);


function [ h ] = tapas_rdcm_get_convolution_bm(options)
% [ h ] = tapas_rdcm_get_convolution_bm(options)
%
% Creates a fixed hemodynamic response function (HRF) by convolving a single
% event (impulse) with the standard Balloon model from DCM.
%
%   Input:
%       options     - options structure with relevant information
%
%   Output:
%       h           - hemodynamic response function (HRF)
%

% ----------------------------------------------------------------------
%
% Authors: Stefan Fraessle (stefanf@biomed.ee.ethz.ch), Ekaterina I. Lomakina
%
% Copyright (C) 2016-2022 Translational Neuromodeling Unit
%                         Institute for Biomedical Engineering
%                         University of Zurich & ETH Zurich
%
% This file is part of the TAPAS rDCM Toolbox, which is released under the
% terms of the GNU General Public License (GPL), version 3.0 or later. You
% can redistribute and/or modify the code under the terms of the GPL. For
% further see COPYING or <http://www.gnu.org/licenses/>.
%
% Please note that this toolbox is in an early stage of development. Changes
% are likely to occur in future releases.
%
% ----------------------------------------------------------------------

% compile source code of integrator
tapas_rdcm_compile()

% get the DCM
DCM = options.DCM;

% number of input data points
N = size(DCM.U.u,1);

% check if full convolution should be generated
if isfield(options,'conv_full') & options.conv_full
    r_dt = 1;
else
    r_dt = N/options.conv_length;
end

% construct a dummy DCM
DCM.a    = -1;
DCM.b    = 0;
DCM.c    = 1;
DCM.d    = zeros(1,1,0);
DCM.Ep   = tapas_rdcm_empty_par(DCM);
DCM.Ep.A = DCM.a;
DCM.Ep.C = DCM.c*16;

% setting input of the fake DCM
DCM.U.u = zeros(size(DCM.U.u,1),1);
DCM.U.u(1:r_dt,:) = 1;

% setting additional parameters of the fake DCM
DCM.Y.dt = DCM.U.dt;
DCM.n    = 1;
DCM.v    = N;
DCM.ns   = N;

% create the HRF from the dummy DCM
DCM = tapas_dcm_euler_make_indices(DCM);
y   = tapas_dcm_euler_gen(DCM, DCM.Ep);

% sample the HRF at the sampling rate of the data
h = y(1:r_dt:end,1);
h = h;

end


% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
xg = [5 5]; % goal position
clf
axis([0 10 0 10]);
hold on
xyzlabel
grid on
xc = 5; yc = 5;
N = 8;
radius = 3;

for i=1:N
    th = (i-1)*2*pi/N;
    x0 = [xc+radius*cos(th) yc+radius*sin(th) th+pi/2];

    plot_vehicle(x0, 'r');
    r = sim('sl_drivepoint');
    y = r.find('yout');
    plot(y(:,1), y(:,2));
end
plot(xg(1), xg(2), '*')


function [crfw, priors] = crfPairwiseTrain(imsegs, pvSP, phSP, adjlist, pE, params, edgelen)

global itercount;
itercount = 0;

vlab = {imsegs(:).vert_labels};
hlab = {imsegs(:).horz_labels};
spweight = {imsegs(:).npixels};
for f = 1:numel(imsegs)
    spweight{f} = spweight{f} / sum(spweight{f});
end

% get f1(y1, y2) = log(P(y1|x)) + log(P(y2|x))
% and f2(y1, y2) = I(y1=y2)*log(P(y1=y2|x)) + I(y1~=y2)*log(P(y1~=y2|x))

npair = 0;
for f = 1:numel(adjlist)
    npair = npair + size(adjlist{f}, 1);
end

f1 = zeros(npair, 49);
f2 = zeros(npair, 49);
f3 = zeros(npair, 49);
lab = zeros(npair, 1);
w = zeros(npair, 1);
%nbpair = zeros(npair, 2);

c = 0;
for f = 1:numel(adjlist)
    npairf = size(adjlist{f}, 1);
    pg = [pvSP{f}(:, 1)  repmat(pvSP{f}(:, 2), 1, 5).*phSP{f}  pvSP{f}(:, 3)];
    labf = (vlab{f}==1)*1 + (vlab{f}==2).*(hlab{f}>0).*(hlab{f}+1) + (vlab{f}==3)*7;

    s1 = adjlist{f}(:, 1);
    s2 = adjlist{f}(:, 2);

    edgeperc = zeros(numel(s1), 2);
    nb = zeros(size(pg, 1), 1);
    for k = 1:npairf
        nb(s1(k)) = nb(s1(k))+1;
        nb(s2(k)) = nb(s2(k))+1;
        edgeperc(k, 1) = edgelen{f}(s1(k), s2(k)) / (sum(edgelen{f}(s1(k), :))+sum(edgelen{f}(:, s1(k))));
        edgeperc(k, 2) = edgelen{f}(s1(k), s2(k)) / (sum(edgelen{f}(s2(k), :))+sum(edgelen{f}(:, s2(k))));
        w(c+k) = sum(spweight{f}([s1(k) s2(k)]));
    end
    %nbpair(c+1:c+npairf, :) = [nb(s1) nb(s2)];
%    min(1./edgeperc(:, 1) + 1./edgeperc(:, 2))
%    max(1./edgeperc(:, 1) + 1./edgeperc(:, 2))
    % unaries normalize for number of edges, and pairwise normalize for
    % edge length
    for k1 = 1:7
        for k2 = 1:7
            f1(c+1:c+npairf, (k1-1)*7+k2) = log(pg(s1, k1))./nb(s1) + log(pg(s2, k2))./nb(s2);
            kp = params(k1 + (k2-1)*7, :);
            if k1==k2
                f2(c+1:c+npairf, (k1-1)*7+k2) = ...
                    log(kp(3)./(1+exp(-kp(1)-kp(2)*(log(pE{f})-log(1-pE{f}))))).*...
                    (edgeperc(:, 1) + edgeperc(:, 2));
            else
                f3(c+1:c+npairf, (k1-1)*7+k2) = ...
                    log(kp(3)./(1+exp(-kp(1)-kp(2)*(log(pE{f})-log(1-pE{f}))))).*...
                    (edgeperc(:, 1) + edgeperc(:, 2));
            end
        end
    end

    lab(c+1:c+npairf) = (labf(s1)>0 & labf(s2)>0).*((labf(s1)-1)*7 + labf(s2));

    c = c + npairf;
end

ind = find(lab==0);
lab(ind) = [];
f1(ind, :) = [];
f2(ind, :) = [];
f3(ind, :) = [];
w(ind, :) = [];

w = w/sum(w);
%nbpair(ind, :) = [];

f2 = f2 - repmat(log(sum(exp(f2), 2)), [1 size(f2, 2)]);
f3 = f3 - repmat(log(sum(exp(f3), 2)), [1 size(f3, 2)]);

fnc{1} = f1;
fnc{2} = f2;
fnc{3} = f3;
%disp(numel(lab))

disp('uniform w')
w = ones(size(w))/numel(w);

[p, err] = avep([1 0 0 zeros(1,27)], fnc, lab, w);
disp(num2str([p err]))

initval = [3 0 1 3 -1 -1 -1 0 0 -1 2 -1 -1 -1 -1 2 2 -1 -1 -1 2 2 -2 -1 2 1 -1 2 2 1];
crfw = fminunc(@(x) objective(x, fnc, lab, w), initval, optimset('TolFun', 0.01));

[p, err] = avep(crfw, fnc, lab, w);
disp(num2str([p err]))

priors = priormat(7, crfw(numel(fnc)+1:end));
crfw = crfw(1:numel(fnc));

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

function lpw = objective(w, f, lab, weight)
global itercount;
correctind = [1:numel(lab)]' + (lab-1)*numel(lab);

%w(1:3) = max(w(1:3), 0);

pmat = priormat(7, w(numel(f)+1:end));
lpw = pmat(lab);
tmpz = repmat(pmat', [numel(lab) 1]);
for k = 1:numel(f)
    lpw = lpw + w(k)*f{k}(correctind);
    tmpz = tmpz + w(k)*f{k};
end
z = log(sum(exp(tmpz), 2));

lpw = -sum((lpw - z).*weight);

itercount = itercount + 1;
if mod(itercount, 25)==0
    disp(num2str([lpw w pmat(end)]))
end

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

function [p err] = avep(w, f, lab, weight)

correctind = [1:numel(lab)]' + (lab-1)*numel(lab);

%w(1:3) = max(w(1:3), 0);

pmat = priormat(7, w(numel(f)+1:end));
lpw = pmat(lab);
tmpz = repmat(pmat', [numel(lab) 1]);
for k = 1:numel(f)
    lpw = lpw + w(k)*f{k}(correctind);
    tmpz = tmpz + w(k)*f{k};
end
z = log(sum(exp(tmpz), 2));

p = sum(exp(lpw-z).*weight);

[tmp, guess] = max(tmpz, [], 2);
err = sum((guess~=lab).*weight);

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

function pmat = priormat(nlab, param);

param(end+1) = -sum(param); % sum of free params equals zero
pmat = zeros(nlab, nlab);
c=0;
for k1 = 1:nlab
    for k2=k1:nlab
        c = c + 1;
        pmat(k1, k2) = param(c);
        pmat(k2, k1) = param(c);
    end
end
pmat = pmat(:);
pmat(end) = -sum(pmat(1:end-1));