function [node,elem,bdFlag,HB] = uniformrefine3(node,elem,bdFlag,HB)
%% UNIFORMREFINE3 uniformly refine a 3-D triangulation.
%
% [node,elem] = uniformrefine3(node,elem) divides each tetrahedra into
% eight small similar sub-tetrahedrons.
%
% [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag) also update boundary
% conditions represented by bdFlag.
%
% [node,elem,~,HB] = uniformrefine3(node,elem) outpus HB array which is
% useful for nodal interpolation. Unlike bisect3, HB is not useful for the
% coarsening. See uniformcoarsen3red.
%
% Warning: uniform refine in 3D is not orientation presereved. Some volumes
% of the sub-tetrahedron are negative. The ordering is special such that
% the refinement can be easy. See uniformrefine3doc for details.
%
% Example
%
%     [node,elem] = cubemesh([-1,1,-1,1,-1,1]);
%     figure(1); subplot(1,3,1);
%     set(gcf,'Units','normal'); set(gcf,'Position',[0.25,0.25,0.5,0.3]);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%     [node,elem] = uniformrefine3(node,elem);
%     figure(1); subplot(1,3,2);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%     bdFlag = setboundary3(node,elem,'Dirichlet');
%     [node,elem,~,bdFlag] = uniformrefine3(node,elem,[],bdFlag);
%     figure(1); subplot(1,3,3);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%
% See also uniformbisect, uniformrefine, bisect, bisect3, uniformcoarsen3red
%
% Reference page in Help browser
%  ifem uniformrefine3doc
%
% Reference: J. Bey. Simplicial grid refinement: on Freudenthal's algorithm
% and the optimal number of congruence classes. Numer. Math.. 85(1):1--29,
% 2000. p11 Algorithm: RedRefinement3D.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('bdFlag','var'), bdFlag =[]; end
if ~exist('HB','var'),    HB = [];     end

%% Construct data structure
[elem2dof,edge] = dof3P2(elem);
N = size(node,1); NT = size(elem,1); NE = size(edge,1);

%% Add new nodes
node(N+1:N+NE,:) = (node(edge(:,1),:)+node(edge(:,2),:))/2;
if ~isempty(HB)
    maxgeneration = max(HB(:,4));
    HB(N+1:N+NE,[1 2 3]) = [(N+1:N+NE)', edge(:,1:2)];
    HB(N+1:N+NE,4) = maxgeneration + 1;
end

%% Refine each tetrahedron into 8 tetrahedrons
t = 1:NT;
p(t,1:10) = elem2dof;
elem(8*NT,:) = [0 0 0 0]; % enlarge the elem array
elem(t,:) = [p(t,1), p(t,5), p(t,6), p(t,7)];
elem(NT+1:2*NT,:) = [p(t,5), p(t,2), p(t,8), p(t,9)];
elem(2*NT+1:3*NT,:) = [p(t,6), p(t,8), p(t,3), p(t,10)];
elem(3*NT+1:4*NT,:) = [p(t,7), p(t,9), p(t,10), p(t,4)];
% always use diagonal 6-9. The ordering is important. See the reference.
elem(4*NT+1:5*NT,:) = [p(t,5), p(t,6), p(t,7), p(t,9)];
elem(5*NT+1:6*NT,:) = [p(t,5), p(t,6), p(t,8), p(t,9)];
elem(6*NT+1:7*NT,:) = [p(t,6), p(t,7), p(t,9), p(t,10)];
elem(7*NT+1:8*NT,:) = [p(t,6), p(t,8), p(t,9), p(t,10)];

%% Update boundary edges
if ~isempty(bdFlag)
    bdFlag(8*NT,:) = [0 0 0 0]; % enlarge the bdFlag array
    bdFlag(NT+1:2*NT,[1 3 4]) = bdFlag(t,[1 3 4]);
    bdFlag(2*NT+1:3*NT,[1 2 4]) = bdFlag(t,[1 2 4]);
    bdFlag(3*NT+1:4*NT,[1 2 3]) = bdFlag(t,[1 2 3]);
    % always use diagonal 6-9
    bdFlag(4*NT+1:5*NT,2) = bdFlag(t,3);
    bdFlag(5*NT+1:6*NT,4) = bdFlag(t,4);
    bdFlag(6*NT+1:7*NT,3) = bdFlag(t,2);
    bdFlag(7*NT+1:8*NT,1) = bdFlag(t,1);
    % change t in the last
    bdFlag(t,1) = 0;
else
    bdFlag = [];
end


function [CP2,CNTmat,TOTmat,LENmat] = zero_crossing(M,T,XX,ZIU,ZIL,Z,mu)
% Estimate out of control points and change-point using zero-crossing
% method.
%
% :Usage:
% ::
%
%     [CP2,CNTmat,TOTmat,LENmat] = zero_crossing(M,T,XX,ZIU,ZIL,Z,mu)
%
% Used in ewma5.m  See ewma5 for description of variables.

CP2 = NaN .* zeros(M,1); % Estimation via Zero-crossings
CNTmat  = zeros(M,1);
TOTmat = zeros(M,1);
LENmat = zeros(M,T);

wh = all(XX' - repmat(mean(XX'),size(XX,2),1) <= eps) | any(isnan(XX),2)';
wh = find(~wh);

ooc = ZIU - ZIL;    % pos if occ +, neg if ooc -, zero otherwise
for d=wh,

    % Calculate last 'zero-crossing'

    amiout = find(ooc(d,:));
    if isempty(amiout)
        amiout = 0;
    else
        amiout = amiout(1);
        amiout = ooc(d,amiout);  % first ooc tp, -1 or 1
    end

    switch amiout

        case 1
            dat = Z(d,:)-mu(d);
            tmp = ZIU(d,:);
            [a,b] = max((conv([1 1 1],tmp) == 3));  % must be OOC 3 consecutive points
            [a,zc] = max((dat(1:b) < 0).*(1:b));
            CP2(d) = zc;

        case -1
            dat = Z(d,:)-mu(d);
            tmp = ZIL(d,:);
            [a,b] = max((conv([1 1 1],tmp) == 3));  % must be OOC 3 consecutive points
            [a,zc] = max((dat(1:b) > 0).*(1:b));
            CP2(d) = zc;

        case 0
            CP2(d) = NaN;
            tmp = 0 .* ZIL(d,:);
        otherwise
            error('This should never happen'),keyboard
    end

    % Calculate number of out-of-control runs and their widths

    [cnt tot len] = cnt_runs(tmp);
    CNTmat(d) = cnt;
    TOTmat(d) = tot;
    LENmat(d,:) = len';

end;

CP2(find(CP2 == 1)) = NaN;

return


%%
col = {[0 114 178],[0 158 115], [213 94 0],[230 159 0],...
    [86 180 233], [204 121 167], [64 224 208], [240 228 66]};

%% AR(1)
g = .95;
[Y, trueC, trueSpikes] = gen_data();
N = size(Y, 1);

% run deconvolution
lam = 2.4;
[c_oasis, s_oasis] = oasisAR1(Y(1,:), g, lam);
[c_foopsi, s_foopsi] = foopsi(Y(1,:), g, lam);

% plot results
figure('papersize', [15, 4]);
init_fig;

% c
axes('position', [.05, .57, .95, .37]);
hold on;
plot(Y(1,:), 'color', col{8}/255);
alpha(.7);
plot(trueC(1,:), 'color', col{3}/255, 'linewidth', 1.5);
plot(c_oasis, 'color', col{1}/255);
plot(c_foopsi, '--', 'color', col{7}/255);
axis tight;
xlim([0, 2000]);
set(gca, 'xtick', [0, 25, 50, 75]*30);
set(gca, 'xticklabel', []);
set(gca, 'ytick', 0:2);
ylabel('Fluor.');
box off;
legend('Data', 'Truth', 'OASIS', 'CVX', 'location', 'northeast', 'orientation', 'horizontal');
% s
axes('position', [.05, .18, .95, .37]);
hold on;
plot(trueSpikes(1,:), 'color', col{3}/255, 'linewidth', 1.5);
plot(s_oasis, 'color', col{1}/255);
plot(s_foopsi, '--', 'color', col{7}/255);
axis tight;
xlim([0, 2000]);
set(gca, 'xtick', [0, 25, 50, 75]*30);
set(gca, 'xticklabel', get(gca, 'xtick')/30);
set(gca, 'ytick', [0,1]);
xlabel('Time [s]');
ylabel('Activity.');

saveas(gcf, 'fig/traceAR1.pdf');

%% time it
fprintf('\n**************AR 1**************\n');
tic;
for m=1:N
    [c_oasis, s_oasis] = oasisAR1(Y(m,:), g, lam);
end
fprintf('OASIS: %.3f seconds\n', toc);

tic;
for m=1:N
    [c_foopsi, s_foopsi] = foopsi(Y(1,:), g, lam); %#ok<ASGLU>
end
fprintf('FOOPSI: %.3f seconds\n', toc);
fprintf('\n**************AR 1**************\n');

%% AR(2)
% simulation
g = [1.7, -0.712];
sn = 1;
seed = 3;
[Y, trueC, trueSpikes] = gen_data(g, sn, [], [], [], [], [], seed);
[N, T] = size(Y);

% deconvolution
lam = 25;
[c_onnls, s_onnls] = onnls(Y(1,:), g, lam);
[c_foopsi, s_foopsi] = foopsi(Y(1,:), g, lam);
figure('papersize', [15, 4]);
init_fig;

% plot results
figure('papersize', [15, 4]);
init_fig;

% c
axes('position', [.05, .57, .46, .37]);
hold on;
plot(Y(1,:), 'color', col{8}/255, 'linewidth', 0.5);
alpha(.7);
plot(trueC(1,:), 'color', col{3}/255, 'linewidth', 1.5);
plot(c_onnls, 'color', col{1}/255);
plot(c_foopsi, '--', 'color', col{7}/255);
axis tight;
xlim([0, 1200]);
set(gca, 'xtick', 0:300:1500);
set(gca, 'xticklabel', []);
ylim(round([1+min(Y(1,:)), max(Y(1,:))-0.5]));
ylabel('Fluor.');
box off;
% s
axes('position', [.05, .18, .46, .37]);
hold on;
plot(trueSpikes(1,:), 'color', col{3}/255, 'linewidth', 1.5);
plot(s_onnls, 'color', col{1}/255);
plot(s_foopsi, '--', 'color', col{7}/255);
axis tight;
xlim([0, 1200]);
set(gca, 'xtick', 0:300:1500);
set(gca, 'xticklabel', get(gca, 'xtick')/30);
set(gca, 'ytick', [0,1]);
xlabel('Time [s]');
ylabel('Activity.');

%% timeit
fprintf('\n**************AR 2**************\n');
tic;
for m=1:N
    [c_onnls, s_onnls] = onnls(Y(m,:), g, lam);
end
fprintf('online NNLS: %.3f seconds\n', toc);

tic;
for m=1:N
    [c_foopsi, s_foopsi] = foopsi(Y(m,:), g, lam);
end
fprintf('FOOPSI: %.3f seconds\n', toc);
fprintf('\n**************AR 2**************\n');


function [diagnostics,out] = VBA_getDiagnostics(posterior,out)
% derives post-hoc diagnostics of VBA's model inversion
% function [diagnostics,out] = VBA_getDiagnostics(posterior,out)
% Post-hoc diagnostics include: goodness-of-fit metrics, Volterra kernels,
% null model evidence, posterior entropies, etc...
% IN:
%   - posterior,out: VBA's output structures
% OUT:
%   - diagnostics: a structure containing the following fields:
%       .pgx: mean of the prior predictive density
%       .pvy: variance of the prior predictive density
%       .kernels: Volterra kernels (if options.kernelSize>0)
%       .efficiency: posterior entropies
%       .DKL: prior/posterior Kullback-Leibler divergences
%       .LLH0: log-evidence of the null model
%       .MT_x: microtime hidden states time series
%       .MT_gx: microtime predicted data time series
%       .microTime: microtime sampling grid
%       .sampleInd: sub-indexing of the microtime sampling grid
%       .dy: data residuals structure (e.g., autocorrelation...)
%       .dx: states innovations structure
%       .C: posterior correlation matrix

u = out.u;
y = out.y;

% get goodness-tof-fit metrics
try; out.fit; catch; out.fit = VBA_getFit(posterior,out); end

% derive Volterra kernels
if out.dim.n_t>1 && out.options.kernelSize>0
    try
        kernels = VBA_getVolterraKernels(posterior,out);
    catch
        VBA_disp('  *** could not derive kernels!\n',out.options);
        kernels = [];
    end
else
    kernels = [];
end

% get null model (H0) evidence
[LLH0] = VBA_LMEH0(y,out.options);

% Entropies and KL divergences
gsi = find([out.options.sources.type]==0) ;
if isempty(gsi)
    efficiency.sigma = NaN;
    DKL.sigma = NaN;
else
    for iG=1:numel(gsi)
        efficiency.sigma(iG) = -out.suffStat.Ssigma(iG);
        m0 = out.options.priors.a_sigma(iG)/out.options.priors.b_sigma(iG);
        v0 = out.options.priors.a_sigma(iG)/out.options.priors.b_sigma(iG)^2;
        m = posterior.a_sigma(iG)/posterior.b_sigma(iG);
        v = posterior.a_sigma(iG)/posterior.b_sigma(iG)^2;
        DKL.sigma(iG) = VBA_KL(m0,v0,m,v,'Gamma');
    end
end
if out.dim.n > 0 % hidden states and initial conditions
    efficiency.X = -out.suffStat.SX;
    efficiency.X0 = -out.suffStat.SX0;
    if isinf(out.options.priors.a_alpha) && isequal(out.options.priors.b_alpha,0)
        efficiency.alpha = NaN;
        DKL.alpha = NaN;
    else
        efficiency.alpha = -out.suffStat.Salpha;
        m0 = out.options.priors.a_alpha./out.options.priors.b_alpha;
        v0 = out.options.priors.a_alpha./out.options.priors.b_alpha^2;
        m = posterior.a_alpha(end)./posterior.b_alpha(end);
        v = posterior.a_alpha(end)./posterior.b_alpha(end)^2;
        DKL.alpha = VBA_KL(m0,v0,m,v,'Gamma');
    end
    try
        DKL.X = 0;
        for t=1:out.dim.n_t
            IN = out.options.params2update.x{t};
            m0 = out.options.priors.muX(IN,t);
            v0 = out.options.priors.SigmaX.current{t}(IN,IN);
            m = posterior.muX(IN,t);
            v = posterior.SigmaX.current{t}(IN,IN);
            DKL.X = DKL.X + VBA_KL(m0,v0,m,v,'Normal');
        end
    catch
        DKL.X = NaN;
    end
    IN = out.options.params2update.x0;
    m0 = out.options.priors.muX0(IN);
    v0 = out.options.priors.SigmaX0(IN,IN);
    m = posterior.muX0(IN);
    v = posterior.SigmaX0(IN,IN);
    DKL.X0 = VBA_KL(m0,v0,m,v,'Normal');
else
    efficiency.X = NaN;
    efficiency.X0 = NaN;
    efficiency.alpha = NaN;
    DKL.X = NaN;
    DKL.X0 = NaN;
    DKL.alpha = NaN;
end
if out.dim.n_phi > 0 % observation parameters
    efficiency.Phi = -out.suffStat.Sphi;
    IN = out.options.params2update.phi;
    m0 = out.options.priors.muPhi(IN);
    v0 = out.options.priors.SigmaPhi(IN,IN);
    if ~out.options.OnLine
        m = posterior.muPhi(IN);
        v = posterior.SigmaPhi(IN,IN);
    else
        m = posterior.muPhi(IN,end);
        v = posterior.SigmaPhi{end}(IN,IN);
    end
    DKL.Phi = VBA_KL(m0,v0,m,v,'Normal');
else
    efficiency.Phi = NaN;
    DKL.Phi = NaN;
end
if out.dim.n_theta > 0 % evolution parameters
    efficiency.Theta = -out.suffStat.Stheta;
    IN = out.options.params2update.theta;
    m0 = out.options.priors.muTheta(IN);
    v0 = out.options.priors.SigmaTheta(IN,IN);
    if ~out.options.OnLine
        m = posterior.muTheta(IN);
        v = posterior.SigmaTheta(IN,IN);
    else
        m = posterior.muTheta(IN,end);
        v = posterior.SigmaTheta{end}(IN,IN);
    end
    DKL.Theta = VBA_KL(m0,v0,m,v,'Normal');
else
    efficiency.Theta = NaN;
    DKL.Theta = NaN;
end

% get prior predictive density
try
    [muy,Vy] = VBA_getLaplace(u,out.options.f_fname,out.options.g_fname,out.dim,out.options,0,'diag');
catch
    muy =[];
    Vy = [];
end

% get micro-time posterior hidden-states estimates
try
    [MT_x,MT_gx,microTime,sampleInd] = VBA_microTime(posterior,u,out);
catch
    MT_x = [];
    MT_gx = [];
    microTime = 1:out.dim.n_t;
    sampleInd = 1:out.dim.n_t;
end

% get residuals structure: data noise
for iS = 1:numel(out.options.sources)
    % source wise
    ySource = out.options.sources(iS).out ;
    res = out.suffStat.dy(ySource,:);
    if out.options.sources(iS).type==0
        gi = find(iS==find([out.options.sources(:).type]==0));
        iQyt = out.options.priors.iQy(:,gi);
        res = getWeightedResiduals(res,iQyt);
    end
    % remove skipped
    res(out.options.isYout(ySource,:)==1) = NaN ;
    dy(iS).dy = res(:);
    dy(iS).R = VBA_spm_autocorr(res);
    dy(iS).m = VBA_nanmean(dy(iS).dy);
    dy(iS).v = VBA_nanvar(dy(iS).dy);
    [dy(iS).ny,dy(iS).nx] = hist(dy(iS).dy,10);
    dy(iS).ny = dy(iS).ny./sum(dy(iS).ny);
    d = diff(dy(iS).nx);
    d = abs(d(1));
    dy(iS).d = d;
    spgy = sum(exp(-0.5.*(dy(iS).m-dy(iS).nx).^2./dy(iS).v));
    dy(iS).grid = dy(iS).nx(1):d*1e-2:dy(iS).nx(end);
    dy(iS).pg = exp(-0.5.*(dy(iS).m-dy(iS).grid).^2./dy(iS).v);
    dy(iS).pg = dy(iS).pg./spgy;
    if  out.options.sources(iS).type==0
        igs = sum([out.options.sources(1:iS).type]==0);
        shat = posterior.a_sigma(igs)./posterior.b_sigma(igs);
        spgy = sum(exp(-0.5.*shat.*dy(iS).nx.^2));
        dy(iS).pg2 = exp(-0.5.*shat.*dy(iS).grid.^2);
        dy(iS).pg2 = dy(iS).pg2./spgy;
    end
end

% get residuals structure: state noise
if ~isempty(out.suffStat.dx)
    wdx = getWeightedResiduals(out.suffStat.dx,out.options.priors.iQx);
    dx.dx = wdx(:);
    dx.m = mean(dx.dx);
    dx.v = var(dx.dx);
    [dx.ny,dx.nx] = hist(dx.dx,10);
    dx.ny = dx.ny./sum(dx.ny);
    d = diff(dx.nx);
    d = abs(d(1));
    dx.d = d;
    spgy = sum(exp(-0.5.*(dx.m-dx.nx).^2./dx.v));
    dx.grid = dx.nx(1):d*1e-2:dx.nx(end);
    dx.pg = exp(-0.5.*(dx.m-dx.grid).^2./dx.v);
    dx.pg = dx.pg./spgy;
    ahat = posterior.a_alpha(end)./posterior.b_alpha(end);
    spgy = sum(exp(-0.5.*ahat.*dx.nx.^2));
    dx.pg2 = exp(-0.5.*ahat.*dx.grid.^2);
    dx.pg2 = dx.pg2./spgy;
else
    dx.dx = [];
end

% get parameters posterior correlation matrix
if out.dim.n > 0 && isinf(out.options.priors.a_alpha) && isequal(out.options.priors.b_alpha,0)
    S = out.suffStat.ODE_posterior.SigmaPhi;
else
    S = NaN*zeros(out.dim.n+out.dim.n_theta+out.dim.n_phi);
    ind = 0;
    if out.dim.n_phi > 0
        if iscell(posterior.SigmaPhi) % online version
            SP = posterior.SigmaPhi{end};
        else
            SP = posterior.SigmaPhi;
        end
        S(1:out.dim.n_phi,1:out.dim.n_phi) = SP;
        ind = out.dim.n_phi;
    end
    if out.dim.n_theta > 0
        if iscell(posterior.SigmaTheta) % online version
            SP = posterior.SigmaTheta{end};
        else
            SP = posterior.SigmaTheta;
        end
        S(ind+1:ind+out.dim.n_theta,ind+1:ind+out.dim.n_theta) = SP;
        ind = ind + out.dim.n_theta;
    end
    if out.dim.n > 0 && out.options.updateX0
        if iscell(posterior.SigmaX0) % online version
            SP = posterior.SigmaX0{end};
        else
            SP = posterior.SigmaX0;
        end
        S(ind+1:ind+out.dim.n,ind+1:ind+out.dim.n) = SP;
    end
end
C = VBA_cov2corr(S);
C = C + diag(NaN.*diag(C));
tick = [0];
ltick = [];
ticklabel = cell(0,0);
if out.dim.n_phi > 0
    ltick = [ltick,tick(end)+out.dim.n_phi/2];
    tick = [tick,out.dim.n_phi];
    ticklabel{end+1} = 'phi';
end
if out.dim.n_theta > 0
    ltick = [ltick,tick(end)+out.dim.n_theta/2];
    tick = [tick,tick(end)+out.dim.n_theta];
    ticklabel{end+1} = 'theta';
end
if out.dim.n > 0 && out.options.updateX0
    ltick = [ltick,tick(end)+out.dim.n/2];
    tick = [tick,tick(end)+out.dim.n];
    ticklabel{end+1} = 'x0';
end
tick = tick +0.5;
tick = tick(2:end-1);
ltick = ltick + 0.5;

% wrap up
diagnostics.pgx = reshape(muy,out.dim.p,[]);
diagnostics.pvy = reshape(Vy,out.dim.p,[]);
diagnostics.kernels = kernels;
diagnostics.efficiency = efficiency;
diagnostics.DKL = DKL;
diagnostics.LLH0 = LLH0;
diagnostics.MT_x = MT_x;
diagnostics.MT_gx = MT_gx;
diagnostics.microTime = microTime;
diagnostics.sampleInd = sampleInd;
diagnostics.dy = dy;
diagnostics.dx = dx;
diagnostics.ltick = ltick;
diagnostics.tick = tick;
diagnostics.ticklabel = ticklabel;
diagnostics.C = C;
out.diagnostics = diagnostics;

function wdx = getWeightedResiduals(dx,iQx)
% weigths residuals according to (state/data) precision matrix
wdx = zeros(size(dx));
for t = 1:size(dx,2)
    sqrtiQ = VBA_sqrtm (iQx{t});
    wdx(:,t) = sqrtiQ*dx(:,t);
end


close all;
clear all;
clc;

addpath([cd '/utilies']);
load(['AR_EigenFace']);

%%%%%%%%%%%%%%%%%%%%%%%%
%FDDL parameter
%%%%%%%%%%%%%%%%%%%%%%%%
opts.nClass        =   100;
opts.wayInit       =   'PCA';
opts.lambda1       =   0.005;
opts.lambda2       =   0.05;
opts.nIter         =   15;
opts.show          =   true;
[Dict,Drls,CoefM,CMlabel] = FDDL(tr_dat,trls,opts);

%%%%%%%%%%%%%%%%%%%%%%%%
% Sparse Classification
%%%%%%%%%%%%%%%%%%%%%%%%
lambda   =   0.005;
nClass   =   opts.nClass;
weight   =   0.5;

td1_ipts.D    =   Dict;
td1_ipts.tau1 =   lambda;
if size(td1_ipts.D,1)>=size(td1_ipts.D,2)
   td1_par.eigenv = eigs(td1_ipts.D'*td1_ipts.D,1);
else
   td1_par.eigenv = eigs(td1_ipts.D*td1_ipts.D',1);
end

ID   =   [];
for indTest = 1:size(tt_dat,2)
    fprintf(['Totalnum:' num2str(size(tt_dat,2)) 'Nowprocess:' num2str(indTest) '\n']);
    td1_ipts.y          =      tt_dat(:,indTest);
    [opts]              =      IPM_SC(td1_ipts,td1_par);
    s                   =      opts.x;

    for indClass  =  1:nClass
        temp_s            =  zeros(size(s));
        temp_s(indClass==Drls) = s(indClass==Drls);
        zz                =  tt_dat(:,indTest)-td1_ipts.D*temp_s;
        gap(indClass)     =  zz(:)'*zz(:);

        mean_coef_c         =   CoefM(:,indClass);
        gCoef3(indClass)    =  norm(s-mean_coef_c,2)^2;
    end

    wgap3  = gap + weight*gCoef3;
    index3 = find(wgap3==min(wgap3));
    id3    = index3(1);
    ID     = [ID id3];
end

fprintf('%s%8f\n','reco_rate  =  ',sum(ID==ttls)/(length(ttls)));


function test15
%TEST15 test cs_amd
%
% Example:
%   test15
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

rand ('state', 0) ;
randn ('state', 0) ;
clf

for trials = 1:100
    n = fix (200 * rand (1)) ;
    d = 0.05 * rand (1) ;
    A = sprandn (n, n, d) ;

    % add a randomly placed dense column
    k = fix (n * rand (1)) ;
    k = max (1, k) ;
    k = min (n, k) ;
    A (:,k) = 1 ;

    try
        p0 = amd (A) ;
    catch
        p0 = symamd (A) ;
    end
    p1 = cs_amd (A) ;

    if (any (sort (p1) ~= 1:n))
        error ('not perm!') ;
    end

    C = A+A' + speye (n) ;
    lnz0 = sum (symbfact (C (p0,p0))) ;
    lnz1 = sum (symbfact (C (p1,p1))) ;
    subplot (1,3,1) ; spy (C)
    subplot (1,3,2) ; spy (C (p0,p0))
    subplot (1,3,3) ; spy (C (p1,p1))
    fprintf ('n %4d nz %6d lnz %6d %6d\n', n, nnz(A), lnz0, lnz1) ;
    drawnow

end


function [price] = PROJ_Double_Barrier(N,alph,call,L,U, S_0,W,M,T,r,rnCHF)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Discrete Double Barrier Options using PROJ method
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
% Author: Justin Lars Kirkby
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% q   = dividend yield (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=1)
% M   = number of subintervals of [0,T] (total of M+1 monitoring points in time grid, including S_0)
% call = 1 for call (else put)
% [L,U] = barriers
% rnCHF = risk netural characteristic function (function handle with single argument)
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% N     = number of grid/basis points (power of 2, e.g. 2^12), resolution = 2*alph/(N-1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

l = log(L/S_0);  u = log(U/S_0);  dt = T/M;

K = N/2;
dx = (u-l)/(K-1);
a  = 1/dx;

E  = ceil(2*alph/(u-l));
if M<= 12
    E = min(E,4);
else
    E = min(E,3);
end

a2    = a^2;
N_Ee  = E*N;
Cons2 = 24*a2*exp(-r*dt)/N_Ee ;

grand = @(w)rnCHF(w).*(sin(w/(2*a))./w).^2./(2+cos(w/a));
dw    = 2*pi*a/N_Ee ;
omega = (dw: dw: (N_Ee -1)*dw);      %We calcuate coefficient of w=0 explicitly
zmin  = (1 - E*K)*dx;           %K corresponds to zero
beta  = Cons2*real(fft([1/(24*a^2) exp(-1i*zmin*omega).*feval(grand,omega)]));

toepM = [beta(E*K:-1:(E-1)*K+1)';0; beta((E+1)*K-1:-1:E*K+1)'];
toepM = fft(toepM);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%STEP 1: Payoff Coefficients
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xmin = l;
nnot = floor(1-xmin*a);   %index to left of x_0=0
lws = log(W/S_0);
nbar = floor(a*(lws - xmin)+1);
rho   = lws - (xmin+(nbar - 1)*dx);
zeta  = a*rho;
xnbar = xmin + (nbar - 1)*dx;

Thet  = zeros(K,1);
Cons3 = 1/48;
Cons4 = 1/12;

%%%%%%  Gaussian Quad Constants
q_plus = (1 + sqrt(3/5))/2;  q_minus = (1 - sqrt(3/5))/2;
b3  = sqrt(15); b4 = b3/10;

%%%% PAYOFF CONSTANTS-----------------------------------
varthet_01 = exp(.5*dx)*(5*cosh(b4*dx) - b3*sinh(b4*dx) + 4)/18;
varthet_m10 = exp(-.5*dx)*(5*cosh(b4*dx) + b3*sinh(b4*dx) + 4)/18;
varthet_star = varthet_01 + varthet_m10;

if call==1  %DBC
    sigma = 1 - zeta; sigma_plus = (q_plus-.5)*sigma; sigma_minus = (q_minus-.5)*sigma;
    es1 = exp(dx*sigma_plus); es2 = exp(dx*sigma_minus);
    dbar_0 = .5 + zeta*(.5*zeta-1);
    dbar_1 = sigma*(1 - .5*sigma);

    d_0 = exp((rho+dx)*.5)*sigma^2/18*(5*((1-q_minus)*es2 +(1-q_plus)*es1) + 4);
    d_1 = exp((rho-dx)*.5)*sigma/18*(5*( (.5*(zeta+1) +sigma_minus)*es2 + (.5*(zeta+1) +sigma_plus)*es1 ) + 4*(zeta+1) );

    Thet(nbar)         = W*(exp(-rho)*d_0 - dbar_0);
    Thet(nbar + 1)     = W*(exp(dx-rho)*(varthet_01 +d_1) -(.5 + dbar_1) );
    Thet(nbar + 2:K-1) = exp(xmin +dx*(nbar+1:K-2))*S_0*varthet_star - W;
    Thet(K)            = U*varthet_m10 - W/2;
    %%%%%%%
    p = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
    Val = p(1:K);
    %%%%%%%
    for m=M-2:-1:0
        Thet(1)      = Cons3*(13*Val(1)+15*Val(2)-5*Val(3)+Val(4));
        Thet(K)      = Cons3*(13*Val(K)+15*Val(K-1)-5*Val(K-2)+Val(K-3));
        Thet(2:K -1) = Cons4*(Val(1:K-2)+10*Val(2:K-1)+Val(3:K));
        %%%%%%%
        p        = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
        Val = p(1:K);
    end

else  %DBP
    zeta_plus = zeta*q_plus; zeta_minus = zeta*q_minus;
    rho_plus = rho*q_plus; rho_minus = rho*q_minus;

    ed1 = exp(rho_minus); ed2 = exp(rho/2); ed3 = exp(rho_plus);

    dbar_1 = zeta^2/2;
    dbar_0 = zeta - dbar_1;         %  dbar_1 = zeta + .5*((zeta - 1)^2 - 1);
    d_0    = zeta*(5*( (1-zeta_minus)*ed1 + (1-zeta_plus)*ed3 ) + 4*(2-zeta)*ed2)/18;
    d_1    = zeta*( 5*(zeta_minus*ed1 + zeta_plus*ed3) + 4*zeta*ed2 )/18;

    Thet(1)        =  W/2 - L*varthet_01;
    Thet(2:nbar-1) =  W - exp(xmin +dx*(1:nbar-2))*S_0*varthet_star;
    Thet(nbar)     =  W*(.5 + dbar_0 - exp(-rho)*(varthet_m10 + d_0));
    Thet(nbar + 1) =  W*(dbar_1 - exp(- rho)*d_1);
    %%%%%%%
    p   = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
    Val = p(1:K);
    %%%%%%%
    for m=M-2:-1:0
        Thet(1)      = Cons3*(13*Val(1)+15*Val(2)-5*Val(3)+Val(4));
        Thet(K)      = Cons3*(13*Val(K)+15*Val(K-1)-5*Val(K-2)+Val(K-3));
        Thet(2:K -1) = Cons4*(Val(1:K-2)+10*Val(2:K-1)+Val(3:K));
        %%%%%%%
        p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
        Val  = p(1:K);
    end
end

xnot = l+(nnot-1)*dx;
xs = [xnot-2*dx,xnot-dx,xnot,xnot+dx,xnot+2*dx];
ys = [Val(nnot-2),Val(nnot-1),Val(nnot),Val(nnot+1),Val(nnot+2)];
price = spline(xs,ys,0);

end


%% FM Comparison (with SHOT descriptors) - First run preprocess_artist_model.m
clear all; close all; clc

addpath(genpath('./'))
addpath(genpath('./../Tools/'))

model_0 = load('./tf_artist/model_0.mat');
model_1 = load('./tf_artist/model_1.mat');
A = model_0.model_evecs_trans*model_0.model_shot;
B = model_1.model_evecs_trans*model_1.model_shot;
C = mldivide(A',B')'; %B = C*A
P = model_1.model_evecs*C*model_0.model_evecs_trans;
P = normc(P);
[~, matches_0_1] = max(P,[],1);

mesh_0 = load('./artist_models/model_0_remesh'); mesh_0 = mesh_0.part;
mesh_1 = load('./artist_models/model_1_remesh'); mesh_1 = mesh_1.model;
D_0 = load('./tf_artist/model_0_dist.mat'); D_0 = D_0.D;
D_1 = load('./tf_artist/model_1_dist.mat'); D_1 = D_1.D;

colors = create_colormap(mesh_1,mesh_1);
figure;
subplot(1,2,1); colormap(colors);
plot_scalar_map(mesh_1,[1: size(mesh_1.VERT,1)]');freeze_colors;title('Target');

subplot(1,2,2); colormap(colors(matches_0_1,:));
plot_scalar_map(mesh_0,[1: size(mesh_0.VERT,1)]');freeze_colors;title('Source');

save('matches_axiomatic_SHOT_FM.mat','matches_0_1');

%% PMF Refinement
[~, matches_filter] = ...
    my_mfilter(mesh_0.n, mesh_1.n, matches_0_1 , [], 150000/400, D_0, D_1, 0); %corr is not used and set to [];

figure;
subplot(1,2,1); colormap(colors);
plot_scalar_map(mesh_1,[1: size(mesh_1.VERT,1)]');freeze_colors;title('Target');

subplot(1,2,2); colormap(colors(matches_filter,:));
plot_scalar_map(mesh_0,[1: size(mesh_0.VERT,1)]');freeze_colors;title('Source');

save('matches_axiomatic_SHOT_FM_PMF.mat','matches_filter');


% SYNTAX:
% dc = hmrR_OD2Conc_new_Nirs( dod, SD, ppf )
%
% UI NAME:
% OD_to_Conc
%
% DESCRIPTION:
% Convert OD to concentrations
%
% INPUTS:
% dod: the change in OD (#time points x #channels)
% SD:  the SD structure
% ppf: Partial path length factors for each wavelength. If there are 2 wavelengths
%      of data, then this is a vector of 2 elements.  Typical value is ~6 for each
%      wavelength if the absorption change is uniform over the volume of tissue measured.
%      To approximate the partial volume effect of a small localized absorption change
%      within an adult human head, this value could be as small as 0.1. Convention is
%      becoming to set ppf=1 and to not divide by the source-detector separation such that
%      the resultant "concentration" is in units of Molar mm (or Molar cm if those are the
%      spatial units). This is becoming wide spread in the literature but there is no
%      fixed citation. Use a value of 1 to choose this option.
%
% OUTPUTS:
% dc: the concentration data (#time points x 3 x #SD pairs
%     3 concentrations are returned (HbO, HbR, HbT)
%
% USAGE OPTIONS:
% Delta_OD_to_Conc: dc = hmrR_OD2Conc_new_Nirs( dod, SD, ppf )
%
% PARAMETERS:
% ppf: [1.0, 1.0, 1.0]
%
function dc = hmrR_OD2Conc_new_Nirs( dod, SD, ppf )

dc = [];
nWav = length(SD.Lambda);
ml = SD.MeasList;

if length(ppf) < nWav
    errordlg('WARNING: Data contains more than 3 wavelengths. Using PPF value of 1 for all wavelengths.');
    ppf = ones(1, nWav);
elseif length(ppf) > nWav
    d = length(ppf)-nWav;
    ppf(end-d+1:end) = [];
end

if ~isempty(find(ppf==1))
    ppf = ones(size(ppf));
end

nTpts = size(dod,1);

e = GetExtinctions( SD.Lambda );
if ~isfield(SD,'SpatialUnit')
    e = e(:,1:2) / 10; % convert from /cm to /mm
elseif strcmpi(SD.SpatialUnit,'mm')
    e = e(:,1:2) / 10; % convert from /cm to /mm
elseif strcmpi(SD.SpatialUnit,'cm')
    e = e(:,1:2) ;
end
einv = inv( e'*e )*e';

lst = find( ml(:,4)==1 );
for idx=1:length(lst)
    idx1 = lst(idx);
    idx2 = find( ml(:,4)>1 & ml(:,1)==ml(idx1,1) & ml(:,2)==ml(idx1,2) );
    rho = norm(SD.SrcPos(ml(idx1,1),:)-SD.DetPos(ml(idx1,2),:));
    if ppf(1)~=1
        dc(:,:,idx) = ( einv * (dod(:,[idx1 idx2'])./(ones(nTpts,1)*rho*ppf))' )';
    else
        dc(:,:,idx) = ( einv * (dod(:,[idx1 idx2'])./(ones(nTpts,1)))' )';
    end
end
dc(:,3,:) = dc(:,1,:) + dc(:,2,:);


function S = MSAFinit(w0,mu,N,L,H,F)

% MSAFinit      Initialize Parameter Structure for the MSAF Algorithm
%                 Psuedo-QMF CMFB is Used by Default, H and F are Used Otherwise
%
% Arguments:
% w0            Coefficients of FIR filter at start
% mu            Step size
% N             Number of subbands
% H             Analysis filter bank (optional), each column represents a filter
% F             Synthesis filter bank (optional), each column represents a filter

if nargin > 4
    if (size(H,2)~=N)|(size(F,2)~=N)
        error('Columns of H (%d) or F (%d) not match with N = %d',size(H,2),size(F,2),N);
    end
else

% Defualt filter bank: Pseudo-QMF CMFB

    [hopt,passedge] = opt_filter(L-1,N); % Generate a prototype lowpass filter
    [H,F] = make_bank(hopt,N);           % Generate filter banks using cosine modulation
    H = sqrt(N)*H';                      % Analysis section
    F = sqrt(N)*F';                      % Synthesis section
end

% Assign structure fields

S.coeffs        = w0(:);                 % Convert to column vector of length M
S.step          = mu;
S.analysis      = H;
S.synthesis     = F;
S.iter          = 0;                     % Iteration count
S.alpha         = ones(1,N)*1e-4;        % Small positive constant
S.AdaptStart    = L + length(w0);        % Running effect of analysis and adaptive filter,
                                         %   minimum L + M

github-repos/MATLAB/CharlesThaCat-acoustic-interference-cancellation/acoustic-interference-cancellation-edb394499ea6f9c96445a3e9613bd64a854c289e/Subband processing/Book/After reading Chapter 6 -- Multiband-structured subband adaptive filters/MSAFinit.m


function [inside] = bounding_mesh(pos, pnt, tri);

% BOUNDING_MESH determines if a point is inside/outside a triangle mesh
% whereby the bounding triangle mesh should be closed.
%
% [inside] = bounding_mesh(pos, pnt, tri)
%
% where
%   pos     position of point of interest (can be 1x3 or Nx3)
%   pnt     bounding mesh vertices
%   tri     bounding mesh triangles
%
% See also SOLID_ANGLE

% Copyright (C) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: bounding_mesh.m 2885 2011-02-16 09:41:58Z roboos $

global fb;
if isempty(fb)
  fb = 0;
end

npos = size(pos, 1);
npnt = size(pnt, 1);
ntri = size(tri, 1);

% determine a cube that encompases the boundary triangulation
bound_min = min(pnt);
bound_max = max(pnt);

% determine a sphere that is completely inside the boundary triangulation
bound_org = mean(pnt);
bound_rad = sqrt(min(sum((pnt - repmat(bound_org, size(pnt,1), 1)).^2, 2)));

inside = zeros(npos, 1);
for i=1:npos
  if fb
    fprintf('%6.2f%%', 100*i/npos);
  end
  if any(pos(i,:)<bound_min) || any(pos(i,:)>bound_max)
    % the point is outside the bounding cube
    inside(i) = 0;
    if fb, fprintf(' outside the bounding cube\n'); end
  elseif sqrt(sum((pos(i,:)-bound_org).^2, 2))<bound_rad
    % the point is inside the interior sphere
    inside(i) = 1;
    if fb, fprintf(' inside the interior sphere\n'); end
  else
    % the point is inside the bounding cube but outside the interior sphere
    % compute the total solid angle of the surface, which is zero for a point outside
    % the triangulation and 4*pi or -4*pi for a point inside (depending on the triangle
    % orientation)
    tmp = pnt - repmat(pos(i,:), npnt, 1);
    solang = solid_angle(tmp, tri);
    if any(isnan(solang))
      inside(i) = nan;
    elseif (abs(sum(solang))-2*pi)<0
      % total solid angle is (approximately) zero
      inside(i) = 0;
    elseif (abs(sum(solang))-2*pi)>0
      % total solid angle is (approximately) plus or minus 4*pi
      inside(i) = 1;
    end
    if fb, fprintf(' solid angle\n'); end
  end
end


function linpack_c_test33 ( )

%*****************************************************************************80
%
%% TEST33 tests CSPFA and CSPDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' )
  fprintf ( 1, 'TEST33\n' );
  fprintf ( 1, '  For a single precision complex (C)\n' );
  fprintf ( 1, '  symmetric matrix in packed storage (SP)\n' );
  fprintf ( 1, '  CSPFA factors the matrix.\n' );
  fprintf ( 1, '  CSPDI computes the determinant or inverse.\n' );
  fprintf ( 1, '\n' )
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the packed matrix A.
%
  k = 0;
  seed = 123456789;

  for j = 1 : n

    for i = 1 : j-1
      k = k + 1;
      [ a(k), seed ] = c4_uniform_01 ( seed );
    end

    k = k + 1;
    [ a(k), seed ] = c4_uniform_01 ( seed );

  end
%
%  Copy the packed matrix into a "normal" matrix.
%
  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      a_save(i,j) = a(k);
    end
  end

  for j = 1 : n
    a_save(j+1:n,j) = transpose ( a_save(j,j+1:n) );
  end

  fprintf ( 1, '\n' )
  fprintf ( 1, '  The matrix A is\n' );
  fprintf ( 1, '\n' )

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a_save(i,j) ), imag ( a_save(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, info ] = cspfa ( a, n );

  if ( info ~= 0 )
    fprintf ( 1, '\n' )
    fprintf ( 1, '  CSPFA returned an error flag INFO = %d\n', info );
    return
  end
%
%  Get the determinant.
%
  job = 10;
  [ a, det ] = cspdi ( a, n, ipvt, job );

  fprintf ( 1, '\n' )
  fprintf ( 1, '  Determinant = (%8f  %8f)*10^(%8f)\n', ...
    real ( det(1) ), imag ( det(1) ), real ( det(2) ) );
%
%  Get the inverse.
%
  job = 01;
  [ a, det ] = cspdi ( a, n, ipvt, job );
%
%  Copy the packed matrix into a "normal" matrix.
%
  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      b_save(i,j) = a(k);
    end
  end

  for j = 1 : n
    b_save(j+1:n,j) = transpose ( b_save(j,j+1:n) );
  end

  c(1:n,1:n) = b_save(1:n,1:n) * a_save(1:n,1:n);

  fprintf ( 1, '\n' )
  fprintf ( 1, '  The product inv(A) * A is\n' );
  fprintf ( 1, '\n' )

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( c(i,j) ), imag ( c(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


%% FEM2D_SCRIPT carries out the finite element assembly and solution procedures.
%
%  Discussion:
%
%    The BATCH command runs scripts, not functions.  So we have to write
%    this short script if we want to work with BATCH!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_SCRIPT\n' );
  fprintf ( 1, '  Call FEM2D_FUN to set up and solve the system.\n' );
%
%  Get the problem parameters.
%
  param = p_data ( );
%
%  Set up the mesh, the linear system, and solve.
%
  [ x, z_ss ] = fem2d_fun ( param );


%% Armstrong no

function n= is_armstrong(num)
if(num<0)
    n=0; %as negative no can't be armstrong
end
sum=0;
temp=num;
while(temp>0)
    sum=sum*10 + rem(temp,10);
    temp=fix(temp/10);
end
if(sum==num)
    n=1;    % the no is armstrong
else
    n=0;    % the no is not armstrong
end
end


function varargout = pnopt_backtrack( x, d, t, f_x, h_x, dg_x, smoothF, ...
  nonsmoothF, desc_param, xtol, maxIter )
% pnopt_backtrack : Backtracking line search for step that satisfies a sufficient
%   descent condition.
%
%   $Revision: 0.8.0 $  $Date: 2012/12/01 $
%
% --------------------Initialize--------------------

  % Set line search parameters
  beta = 0.5;

  % Set termination flags
  FLAG_SUFFDESC = 1;
  FLAG_TOLX     = 2;
  FLAG_MAXFUNEV = 3;

  iter = 0;

  desc = dg_x + nonsmoothF( x + d ) - h_x;

  % --------------------Main Loop--------------------
  while 1
    iter = iter + 1;

    % Evaluate trial point and function value.
    y = x + d * t;
    if nargout > 6
      [ g_y, Dg_y, D2g_y ] = smoothF( y );
    else
      [ g_y, Dg_y ] = smoothF( y );
    end
    h_y = nonsmoothF( y );
    f_y = g_y + h_y;

    % Check termination criteria
    if f_y < f_x + desc_param * t * desc         % Sufficient descent condition satisfied
      flag = FLAG_SUFFDESC;
      break
    elseif t <= xtol            % Step length too small
      flag = FLAG_TOLX;
      break
    elseif iter >= maxIter      % Too many line search iterations
      flag = FLAG_MAXFUNEV;
      break
    end

    % Backtrack if objective value not well-defined
    if isnan( f_y ) || isinf( f_y ) || abs( f_y - f_x - t * dg_x ) <= 1e-9
      t = beta * t;
    % Safeguard quadratic interpolation
    else
      t_interp = - ( dg_x * t^2) / ( 2 * ( f_y - f_x - t * dg_x ) );
      if 0.01 <= t_interp && t_interp <= 0.99*t
        t = t_interp;
      else
        t = beta * t;
      end
    end

  end

  if nargout > 6
    varargout = { y, f_y, Dg_y, D2g_y, t, flag ,iter };
  else
    varargout = { y, f_y, Dg_y, t, flag ,iter };
  end


function b = isPointInPolygon(point, poly)
%ISPOINTINPOLYGON Test if a point is located inside a polygon
%
%   B = isPointInPolygon(POINT, POLYGON)
%   Returns true if the point is located within the given polygon.
%
%   This function is simply a wrapper for the function inpolygon, to avoid
%   decomposition of point and polygon coordinates.
%
%   Example
%     pt1 = [30 20];
%     pt2 = [30 5];
%     poly = [10 10;50 10;50 50;10 50];
%     isPointInPolygon([pt1;pt2], poly)
%     ans =
%          1
%          0
%
%     poly = [0 0; 10 0;10 10;0 10;NaN NaN;3 3;3 7;7 7;7 3];
%     pts = [5 1;5 4];
%     isPointInPolygon(pts, poly);
%     ans =
%          1
%          0
%
%
%   See also
%   points2d, polygons2d, inpolygon, isPointInTriangle

%
% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2009-06-19,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

%   HISTORY
%   2013-04-24 add support for multiply connected polygons

% In case of a multiple polygon, decompose into a set of contours, and
% performs test for each contour
if iscell(poly) || any(isnan(poly(:)))
    % transform as a cell array of simple polygons
    polygons = splitPolygons(poly);
    N = length(polygons);
    Np = size(point, 1);

    % compute orientation of polygon, and format to have Np*N matrix
    areas = zeros(N, 1);
    for i = 1:N
        areas(i) = polygonArea(polygons{i});
    end
    ccw = areas > 0;
    ccw = repmat(ccw', Np, 1);

    % test if point inside each polygon
    in = false(size(point, 1), N);
    for i = 1:N
        poly = polygons{i};
        in(:, i) = inpolygon(point(:,1), point(:,2), poly(:,1), poly(:,2));
    end

    % count polygons containing point, weighted by polygon orientation
    b = sum(in.*(ccw==1) - in.*(ccw==0), 2) > 0;

else
    % standard test for simple polygons
    b = inpolygon(point(:,1), point(:,2), poly(:,1), poly(:,2));
end


function tv = Fval(u, img, alpha, huber)
% total variation part of the criterion

[H W] = size(img);
N = W * H;

nabla = make_derivatives_mine(H, W);

if huber
    tv = nabla * u(:);
    tv_size = sqrt(tv(1:N).^2 + tv(N+1:end).^2);
    idx1 = tv_size <= alpha;
    idx2 = tv_size > alpha;
    tv = sum(tv_size(idx1).^2/(2*alpha)) + sum(tv_size(idx2) - alpha/2);
else
    tv = nabla * u(:);
    tv = sum(sqrt(tv(1:N).^2 + tv(N+1:end).^2));
end


function r8col_to_r8vec_test ( )

%*****************************************************************************80
%
%% R8COL_TO_R8VEC_TEST tests R8COL_TO_R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8COL_TO_R8VEC_TEST\n' );
  fprintf ( 1, '  R8COL_TO_R8VEC converts an array of columns to a vector.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m
    for j = 1 : n
      a(i,j) = 10 * i + j;
    end
  end

  r8mat_print ( m, n, a, '  The array of columns:' );

  x = r8col_to_r8vec ( m, n, a );

  r8vec_print ( m*n, x, '  The resulting vector of columns:' );

  return
end


function Offspring = Operator(Problem,Particle,Pbest,Gbest)
% Particle swarm optimization in MMOPSO

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2,N,1),1,D);
    OffVel        = W.*ParticleVel;
    temp          = repmat(rand(N,1)<0.7,1,D);
    OffVel(temp)  = OffVel(temp) + C1(temp).*r1(temp).*(PbestDec(temp)-ParticleDec(temp));
    OffVel(~temp) = OffVel(~temp) + C2(~temp).*r2(~temp).*(GbestDec(~temp)-ParticleDec(~temp));
    OffDec        = ParticleDec + OffVel;
    Offspring     = Problem.Evaluation(OffDec,OffVel);
end


function [Gc,Kp,Ti,Td,H]=optPID(key,typ,vars)
k=vars(1); L=vars(2); T=vars(3); N=vars(4);
Td=[];H=1;
if length(vars)==5, iC=vars(5); tt=0;
else,
   Kc=vars(5); Tc=vars(6); kappa=vars(7); tt=1;
end
if tt==0
   if key==2
PIDtab=[0.980, 0.712, 0.569, 1.072, 0.786, 0.628;
       -0.892,-0.921,-0.951,-0.560,-0.559,-0.583;
        0.690, 0.968, 1.023, 0.648, 0.883, 1.007;
       -0.155,-0.247,-0.179,-0.114,-0.158,-0.167];
   elseif key==3
PIDtab=[1.048, 1.042, 0.968, 1.154, 1.142, 1.061;
       -0.897,-0.897,-0.904,-0.567,-0.579,-0.583;
        1.195, 0.987, 0.977, 1.047, 0.919, 0.892;
       -0.368,-0.238,-0.253,-0.220,-0.172,-0.165;
        0.489, 0.385, 0.316, 0.490, 0.384, 0.315;
        0.888, 0.906, 0.892, 0.708, 0.839, 0.832];
   elseif key==4
PIDtab=[1.260, 1.053, 0.942, 1.295, 1.120, 1.001;
       -0.887,-0.930,-0.933,-0.619,-0.625,-0.624;
        0.701, 0.736, 0.770, 0.661, 0.720, 0.754;
       -0.147,-0.126,-0.130,-0.110,-0.114,-0.116;
        0.375, 0.349, 0.308, 0.378, 0.350, 0.308;
        0.886, 0.907, 0.897, 0.756, 0.811, 0.813];
   end
   ii=0; if (L/T>1) ii=3; end; tt=L/T;
   a1=PIDtab(1,ii+iC); b1=PIDtab(2,ii+iC);
   a2=PIDtab(3,ii+iC); b2=PIDtab(4,ii+iC);
   Kp=a1/k*tt^b1; Ti=T/(a2+b2*tt);
   if key==3| key==4
      a3=PIDtab(5,ii+iC); b3=PIDtab(6,ii+iC);
      Td=a3*T*tt^b3;
   end
else
   if key==2,
      Kp=0.361*Kc; Ti=0.083*(1.935*kappa+1)*Tc;
   elseif key==3,
      Kp=0.509*Kc; Td=0.125*Tc;
      Ti=0.051*(3.302*kappa+1)*Tc;
   elseif key==4,
      Kp=(4.437*kappa-1.587)...
         /(8.024*kappa-1.435)*Kc;
      Ti=0.037*(5.89*kappa+1)*Tc;
      Td=0.112*Tc;
   end
end
if key==2, Gc=tf(Kp*[Ti,1],[Ti,0]);
elseif key==3
   nn=[Kp*Ti*Td*(N+1)/N, Kp*(Ti+Td/N), Kp];
   dd=Ti*[Td/N,1,0]; Gc=tf(nn,dd);
elseif key==4
   Gc=tf(Kp*[Ti,1],[Ti,0]);
   nH=[(1+Kp/N)*Ti*Td, Kp*(Ti+Td/N), Kp];
   dH=Kp*conv([Ti,1],[Td/N,1]); H=tf(nH,dH);
end


function [R,eff] = randmio_und_connected(R, ITER)
%RANDMIO_UND_CONNECTED     Random graph with preserved degree distribution
%
%   R = randmio_und_connected(W,ITER);
%   [R eff] = randmio_und_connected(W, ITER);
%
%   This function randomizes an undirected network, while preserving the
%   degree distribution. The function does not preserve the strength
%   distribution in weighted networks. The function also ensures that the
%   randomized network maintains connectedness, the ability for every node
%   to reach every other node in the network. The input network for this
%   function must be connected.
%
%   Input:      W,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%
%   Output:     R,      randomized network
%               eff,    number of actual rewirings carried out
%
%   References: Maslov and Sneppen (2002) Science 296:910
%
%
%   2007-2012
%   Mika Rubinov, UNSW
%   Jonathan Power, WUSTL
%   Olaf Sporns, IU

%   Modification History:
%   Jun 2007: Original (Mika Rubinov)
%   Apr 2008: Edge c-d is flipped with 50% probability, allowing to explore
%             all potential rewirings (Jonathan Power)
%   Mar 2012: Limit number of rewiring attempts, count number of successful
%             rewirings (Olaf Sporns)

n=size(R,1);
[i,j]=find(tril(R));
K=length(i);
ITER=K*ITER;

% maximal number of rewiring attempts per 'iter'
maxAttempts= round(n*K/(n*(n-1)));
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)                                     %while not rewired
        rewire=1;
        while 1
            e1=ceil(K*rand);
            e2=ceil(K*rand);
            while (e2==e1)
                e2=ceil(K*rand);
            end
            a=i(e1); b=j(e1);
            c=i(e2); d=j(e2);

            if all(a~=[c d]) && all(b~=[c d])
                break           %all four vertices must be different
            end
        end

        if rand>0.5
            i(e2)=d; j(e2)=c; 	%flip edge c-d with 50% probability
            c=i(e2); d=j(e2); 	%to explore all potential rewirings
        end

        %rewiring condition
        if ~(R(a,d) || R(c,b))
            %connectedness condition
            if ~(R(a,c) || R(b,d))
                P=R([a d],:);
                P(1,b)=0; P(2,c)=0;
                PN=P;
                PN(:,d)=1; PN(:,a)=1;

                while 1
                    P(1,:)=any(R(P(1,:)~=0,:),1);
                    P(2,:)=any(R(P(2,:)~=0,:),1);
                    P=P.*(~PN);
                    if ~all(any(P,2))
                        rewire=0;
                        break
                    elseif any(any(P(:,[b c])))
                        break
                    end
                    PN=PN+P;
                end
            end %connectedness testing

            if rewire               %reassign edges
                R(a,d)=R(a,b); R(a,b)=0;
                R(d,a)=R(b,a); R(b,a)=0;
                R(c,b)=R(c,d); R(c,d)=0;
                R(b,c)=R(d,c); R(d,c)=0;

                j(e1) = d;          %reassign edge indices
                j(e2) = b;
                eff = eff+1;
                break;
            end %edge reassignment
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations


function Q = planarize(V,F,varargin)
  % PLANARIZE Planarize the quads of quad mesh using the local-local technique
  % of "Interactive Planarization and Optimization of 3D Meshes" [Poranne et
  % al. 2012]
  %
  % Q = planarize(V,F,varargin)
  %
  % Inputs:
  %   V  #V by dim list of mesh vertex positions
  %   F  #F by 4 list of quad indices into V
  %     Optional:
  %       'MaxIter' followed by maximum number of iterations {100}
  %       'mu' followed by "mu" penalty value {0.9}
  %       'r' followed by "r" penalty shrink factor {0.9}
  % Outputs:
  %   Q  #V by dim list of new mesh vertex positions
  %

  % default values
  max_iter = 100;
  mu = 0.9;
  r = 0.9;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'MaxIter','mu','r'}, ...
    {'max_iter','mu','r'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  % number of quads
  m = size(F,1);
  % number of vertices
  n = size(V,1);
  dim = 3;
  T = [F(:,[1 2 3]);F(:,[1 3 4])];
  N = normalizerow(normals(V,T));
  % Initial guess for N
  N = 0.5*(N(1:m,:)+N(m+1:end,:));
  A = doublearea(V,T);
  % A sort of barycentric massmatrix for quad meshes
  M = repdiag(sparse(T(:),T(:),0.25*repmat(A,3,1),n,n),dim);
  E = [];

  % nsp = 3;
  % subplot(1,nsp,1);
  % BC = barycenter(V,F);
  % trisurf(F,V(:,1),V(:,2),V(:,3));
  % hold on;
  % quiver3(BC(:,1),BC(:,2),BC(:,3),N(:,1),N(:,2),N(:,3));
  % hold off;

  % "corner" incidence
  C = sparse(F(:),repmat(1:m,1,4)',1,n,m);

  D = zeros(m,1);

  Q = V;
  iter = 1;
  while true

    % LOCAL
    for ni = 1:m
      Qn = Q(F(ni,:),:);
      Mn = mean(Qn,1);
      Qnc = bsxfun(@minus,Qn,Mn);
      [EV,ED] = eig(Qnc'*Qnc);
      [~,mi] = min(diag(ED));
      N(ni,:) = EV(:,mi);
      D(ni) = -Mn*N(ni,:)';
    end

    % LOCAL
    for v = 1:n
      % faces incident on v
      J = find(C(v,:));
      NJ = N(J,:);
      DJ = D(J);
      AJ = mu*eye(3) + (1-mu)*(NJ'*NJ);
      bJ = mu*V(v,:)' - (1-mu)*NJ'*DJ;
      Q(v,:) = AJ\bJ;
    end

    % GLOBAL
    %% "corner" incidence for each dimension with Nj
    %This is wrong becase D should be scalar...
    %CN = sparse( ...
    %  reshape(bsxfun(@plus,F(:),(0:dim-1)*n),dim*m*4,1), ...
    %  reshape(bsxfun(@plus,repmat((1:m)',4,1),(0:dim-1)*m),dim*m*4,1), ...
    %  reshape(repmat(N,4,1),4*m*dim,1), ...
    %  n*dim,m);
    %% μ ‖P-Q‖² + (1-μ)∑_ij(n_j*q_i-d_j)²
    %% Identity
    %I = repdiag(speye(m,m),dim);
    %H = [          mu*M        (1-mu)*CN; ...
    %          (1-mu)*CN'       (1-mu)*I];
    %l = [-mu*2*M*V(:);zeros(m*dim,1)];
    %QD = min_quad_with_fixed(H,l,[],[]);
    %Q = reshape(QD(1:n*dim),n,dim);
    %D = reshape(QD(n*dim+1:end),m,dim);
    %% (1-μ)∑_ij(n_j*q_i-d_j)²
    %% ‖n_i‖² = 1
    %%
    %CQ = sparse( ...
    %  reshape(bsxfun(@plus,F(:),(0:dim-1)*n),dim*m*4,1), ...
    %  reshape(bsxfun(@plus,repmat((1:m)',4,1),(0:dim-1)*m),dim*m*4,1), ...
    %  reshape(Q(F(:),:),m*dim*4,1), ...
    %  n*dim,m*dim);
    %%H = (1-mu)*diag(sum(CQ'.^2,2));
    %%l = (1-mu)*-2*diag(sum(CQ',2))*D(:);
    %%% min N'HN - 2N'l
    %%% ‖N‖² = m

    %subplot(1,nsp,2);
    %BC = barycenter(Q,F);
    %trisurf(F,Q(:,1),Q(:,2),Q(:,3));
    %hold on;
    %quiver3(BC(:,1),BC(:,2),BC(:,3),N(:,1),N(:,2),N(:,3));
    %hold off;

    %NT = normalizerow(normals(Q,T));
    %e = sum(sum((NT(1:m,:)-NT(m+(1:m),:)).^2,2));
    %E = [E(:);e];
    %subplot(1,nsp,3);
    %loglog(E);
    iter = iter+1;
    if iter>max_iter
      break;
    end

    mu = r*mu;
  end
end


function  [im_out, par] = SC_Sigma_1AG(par)
im_out    =   par.nim;
% parameters for noisy image
[h,  w, ch]      =  size(im_out);
par.h = h;
par.w = w;
par.ch = ch;
par = SearchNeighborIndex( par );
for ite  =  1 : par.outerIter
    %     % iterative regularization
    im_out = im_out+par.delta*(par.nim - im_out);
    % image to patches and estimate local noise variance
    Y = Image2PatchNew( im_out, par );
    % estimation of noise variance
    dif = mean( mean( mean( (par.nim-im_out).^2 ) ) );
    par.sigma = sqrt( abs( par.nSig^2 - dif ) );
    % estimation of noise variance
    if mod(ite-1, par.innerIter)==0
        par.nlsp = par.nlsp - par.nlspgap;
        % searching  non-local patches
        blk_arr = Block_Matching( Y, par );
    end
    % Weighted Sparse Coding
    Y_hat = zeros(par.ps2ch, par.maxrc, 'single');
    W_hat = zeros(par.ps2ch, par.maxrc, 'single');
    for i = 1:par.lenrc
        index = blk_arr(:, i);
        nlY = Y( : , index );
        DC = mean(nlY, 2);
        nDCnlY = bsxfun(@minus, nlY, DC);
        % update D and S
        [D, ~, ~] = svd( full(nDCnlY), 'econ' );
        % update C by soft thresholding
        B = D' * nDCnlY;
        C = sign(B) .* max( abs(B) -  par.lambda*par.sigma^2, 0 );
        % update Y
        nDCnlYhat = D * C;
        % add back DC components
        nlYhat = bsxfun(@plus, nDCnlYhat, DC);
        % aggregation
        Y_hat(:, index) = Y_hat(:, index) + nlYhat;
        W_hat(:, index) = W_hat(:, index) + ones(par.ps2ch, par.nlsp);
    end
    % Reconstruction
    im_out = PGs2Image(Y_hat, W_hat, par);
    % calculate the PSNR and SSIM
    PSNR =   csnr( im_out*255, par.I*255, 0, 0 );
    SSIM      =  cal_ssim( im_out*255, par.I*255, 0, 0 );
    fprintf('Iter %d : PSNR = %2.4f, SSIM = %2.4f\n', ite, PSNR, SSIM);
    par.PSNR(ite, par.image) = PSNR;
    par.SSIM(ite, par.image) = SSIM;
end
return;


function [Y_pred, loglikelihoodX] = LDApredict(model,Gamma,X,classification,intercept)
% For an inferred LDA model and state timecourses, computes the
% likelihood manifold in X space and computes the likelihood function for
% each class of labels.
if nargin<4
    classification=true;
end
if nargin<5
    intercept=true;
end
[T2,K] = size(Gamma);
[T,nDimX] = size(X);
nDimY = size(model.state(1).W.Mu_W,2)-nDimX;
betas_mu = cell(K,1);
betas_sigma = cell(K,1);
for k = 1:K
    betas_mu{k} = model.state(k).W.Mu_W((nDimX+1):end,1:nDimX);
    if strcmp(model.train.covtype,'full') || strcmp(model.train.covtype,'uniquefull') ...
            || strcmp(model.train.covtype,'sharedfull')
        S = model.train.S==1;
        betas_sigma{k} = model.state(k).W.S_W(logical(S(:)),logical(S(:)));
    else
        betas_sigma{k} = model.state(k).W.S_W(1:nDimX,(nDimX+1):end,(nDimX+1):end);
    end
end

% Iterate through labels, noting that the distribution in x space can be
% computed as a sum for each condition:
if intercept
    numconds = size(betas_mu{1},1)-1;
else
    numconds = size(betas_mu{1},1);
end
betamu_givenY = zeros(numconds,K,nDimX);
betasig_givenY = zeros(numconds,K,nDimX,nDimX);
for testcond = 1:numconds
    % compute each state's distribution in x space - assuming intercept has
    % been used
    for k=1:K
        if intercept %account for mean term in generative model:
            %betamu_givenY(testcond,k,:) = betas_mu{k}(1,:) + betas_mu{k}(1 + testcond,:);
            betamu_givenY(testcond,k,:) = betas_mu{k}(1 + testcond,:);
        else
            betamu_givenY(testcond,k,:) = betas_mu{k}(testcond,:);
        end
        if strcmp(model.train.covtype,'full') || strcmp(model.train.covtype,'uniquefull') || ...
            strcmp(model.train.covtype,'sharedfull')
            betasig_givenY(testcond,k,:,:) = betas_sigma{k}([1:nDimY:nDimY*nDimX],[1:nDimY:nDimY*nDimX])+...
                betas_sigma{k}([testcond:nDimY:nDimY*nDimX],[testcond:nDimY:nDimY*nDimX]);
        else % Naive bayes classifier - just take uncertainty on each channel
            betasig_givenY(testcond,k,:,:) = diag(betas_sigma{k}(:,testcond + 1,testcond + 1)) + ...
                diag(betas_sigma{k}(:,1,1)) + 2*diag(betas_sigma{k}(:,1,testcond + 1));
        end
    end
end

% iterate through time, computing likelihood of X given Gamma:
if isfield(model,'Omega')
    if isfield(model.Omega,'pseudomean')
        CovMat = model.Omega.pseudomean(1:nDimX,1:nDimX);
    elseif strcmp(model.train.covtype,'uniquediag') || ...
           strcmp(model.train.covtype,'shareddiag')  % Naive Bayes case
        prec = diag(model.Omega.Gam_shape ./ model.Omega.Gam_rate(1:nDimX));
        CovMat = inv(prec);
    else % shared full covariance matrix
        prec = model.Omega.Gam_shape * model.Omega.Gam_irate(1:nDimX,1:nDimX);
        CovMat = inv(prec);
    end
else
    if strcmp(model.train.covtype,'diag')
        for k=1:K
            prec = diag(model.state(k).Omega.Gam_shape ./ model.state(k).Omega.Gam_rate(1:nDimX));
            CovMat(:,:,k) = inv(prec);
        end
    else % statewise full covariance
        for k=1:K
            prec = model.state(k).Omega.Gam_shape * model.state(k).Omega.Gam_irate(1:nDimX,1:nDimX);
            CovMat(:,:,k) = inv(prec);
        end
    end
end

% remove intercept from data if necessary:
if intercept
    for k=1:K
        X = X - Gamma(:,k) * betas_mu{k}(1,:);
    end
    Y_pred = zeros(T,nDimY-1);
else
    Y_pred = zeros(T,nDimY);
end
loglikelihoodX = zeros(size(Y_pred));
if T==T2
    for t=1:T
        CovMat_t = sum(CovMat .* repmat(permute(Gamma(t,:),[3,1,2]),nDimX,nDimX,1),3);
        if classification
            for testcond = 1:numconds
                mu = sum(squeeze(betamu_givenY(testcond,:,:)) .*repmat(Gamma(t,:)',1,nDimX));
                mu_rec(testcond,t,:) = mu;
                S = CovMat_t + squeeze(sum(betasig_givenY(testcond,:,:,:).*repmat(permute(Gamma(t,:)',[3,1,2]),1,1,nDimX,nDimX),2));
                loglikelihoodX(t,testcond) = -0.5*log(det(S)) -0.5 * (X(t,:) - mu) * inv(S) * (X(t,:) - mu)';
            end
            m = max(loglikelihoodX(t,:),[],2);
            Y_pred(t,:) = loglikelihoodX(t,:)==m;
            if sum(Y_pred(t,:))>1
                warning(['Equal scores achieved for multiple classes, t=',int2str(t),'\n\n']);
                a = find(Y_pred(t,:));
                Y_pred(t,a) = 0;
                Y_pred(t,a(randi(length(a)))) = 1; %randomly silence all but one of these entries
            end
        else
            betas_t = permute(sum(repmat(Gamma(t,:),[numconds,1,nDimX]).* betamu_givenY,2),[1,3,2]);

            %alternative implementation:
            if ~intercept
                sigma_Y = inv(inv(eye(nDimY)) + betas_t * inv(CovMat_t) * betas_t');
            else
                sigma_Y = inv(inv(eye(nDimY-1)) + betas_t * inv(CovMat_t) * betas_t');
            end
            betas_backwardmodel = inv(CovMat_t) * betas_t' * sigma_Y;
            Y_pred(t,:) = X(t,:)*betas_backwardmodel;
        end
    end
else
%implies Gamma invariant, so collapse over time:
    for testcond = 1:numconds
        mu = squeeze(betamu_givenY(testcond,:,:))';
        mu_rec(testcond,:) = mu;
        sig = CovMat + squeeze(betasig_givenY(testcond,:,:,:));
        loglikelihoodX(:,testcond) = -0.5 * log(det(sig)) -0.5 * sum(((X-repmat(mu,T,1))*inv(sig)).*(X-repmat(mu,T,1)),2);
    end
    m = max(loglikelihoodX(:,:),[],2);
    Y_pred = loglikelihoodX(:,:)==repmat(m,1,numconds);
end
if numconds==2 && all(Y_pred(:,1) == 1-Y_pred(:,2))
    % binary output:
    Y_pred = Y_pred(:,1);
end
end


function [W] = MW2W(MW)
% Convert power from megawatts to watts.
% Chad A. Greene 2012
W = MW*1000000 ;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2017 Lime Microsystems Ltd.
%
% Licensed under the Apache License, Version 2.0 (the 'License');
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%    http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an 'AS IS' BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function NumptyGraphs(fpltname,iqDataTx,iqDataRxRaw,iqDataRx,binData,din,dout,FSR,osr,msgLen,saveGraphs)
  %
  %   D I S P L A Y   G R A P H I C A L   S U M M A R Y
  %
  subplot(2,3,1);
  plot(iqDataTx(1:((msgLen+1)*osr)),'b');
  xlabel('Time');
  ylabel('Level');
  title('Filtered BPSK Message');
  grid on;
  subplot(2,3,2);
  plot(binData,'go',dout,'rx-');
  legend('transmitted','received');
  xlabel('Time');
  ylabel('Level');
  title('Binary Message');
  grid on;
  subplot(2,3,3);
  fsc=FSR*((-(length(iqDataRx)-1)/2):((length(iqDataRx)-1)/2))/length(iqDataRx);
  fftr=fftshift(mag2db(abs(fft(iqDataTx)/length(iqDataTx))+1e-5));
  plot(fsc,fftr);
  grid on;
	title('Spectrum of RRC Filtered Tx Signal');
	xlabel('Frequency MHz');
	ylabel('Level (dB)');
	axis('tight');
  subplot(2,3,4);
  fftr=fftshift(mag2db(abs(fft(iqDataRxRaw)/length(iqDataRxRaw))+1e-5));
  plot(fsc,fftr);
  grid on;
	title('Spectrum of Raw Rx Signal');
	xlabel('Frequency MHz');
	ylabel('Level (dB)');
	axis('tight');
  subplot(2,3,5);
  fftr=fftshift(mag2db(abs(fft(iqDataRx)/length(iqDataRx))+1e-5));
  plot(fsc,fftr);
  grid on;
	title('Spectrum of RRC Filtered Rx Signal');
	xlabel('Frequency MHz');
	ylabel('Level (dB)');
	axis('tight');
  subplot(2,3,6);
 	plot(iqDataRx,'b-',din,'rx');
	grid on;
	title('Trajectory of RRC Filtered RX signal');
	xlabel('I level');
	ylabel('Q level');
  legend('Traj','Locked');
  if saveGraphs
    print( fpltname, '-dpng' );
  end
end


function [vol, labels] = imVolume(img, varargin)
% Volume of regions within a 3D binary or label image.
%
%   V = imVolume(IMG);
%   Compute volume of the image. IMG is either a binary image, or a label
%   image. In the case of a label image, the area of each labeled area is
%   returned in a column vector with as many elements as the number of
%   labels.
%
%   V = imVolume(IMG, SCALE);
%   Also specify scale of image tile. SCALE si a 1-by-3 array, containing
%   voxel size in each direction.
%
%   See Also
%   imVolumeDensity, imSurfaceArea, imMeanBreadth
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-01-15,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRAE - Cepia Software Platform.

%% Parse input arguments

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D binary or label image');
end

% the labels to compute
labels = [];

% default spatial calibration
delta = [1 1 1];

% parse parameter name-value pairs
while ~isempty(varargin)
    var1 = varargin{1};
    varargin(1) = [];

    if isnumeric(var1)
        % option can be number of directions, spatial calibration, or list
        % of labels
        if all(size(var1) == [1 3])
            % spatial calibration
            delta = var1;
        elseif ~isscalar(var1) && size(var1, 2) == 1
            % list of labels to compute
            labels = var1;
        else
            error('Unable to parse input argument');
        end
    else
        error('Expect numeric input only');
    end
end

%% Process label images

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % extract labels if necessary (considers 0 as background)
    if isempty(labels)
        labels = imFindLabels(img);
    end

    % allocate memory
    vol = zeros(length(labels), 1);

    % iterate over labels
    for i = 1:length(labels)
        vol(i) = imVolume(img==labels(i), delta);
    end
    return;
end

%% Process binary images

% in case of binary image, compute only one label...
labels = 1;

% compute area, multiplied by image resolution
vol = sum(img(:)) * prod(delta);


function b = isint(x)
%ISINT  True for integers.
%
%   ISINT(X) returns 1's where the elements of X are integers and 0's where
%   they are not.  For example, ISINT([ 2e9 pi 3+5i NaN ]) is [ 1 0 1 0 ].
%
%   See also ISEVEN, ISODD.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-04-12 14:30:59 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   error(nargchk(1, 1, nargin));
   if ~isnumeric(x)
      error('Argument must be a numeric array.');
   end

   cls = class(x);                      % class of input argument
   if isempty(x)
      b = feval(cls, x);                % return empty array of same class
   else
      switch cls
         case 'double'
            b = x == round(x);
         case 'single'
            % "mod" is not defined for class "single"; so convert input to
            % double, compare, and convert back
            d = double(x);
            b = single(d == round(d));
         case {'int8', 'int16', 'int32', 'int64', ...
               'uint8', 'uint16', 'uint32', 'uint64'}
            % return an array of ones of the same class as input argument
            b = logical(repmat(feval(cls, 1), size(x)));
         otherwise
            error('Argument is of unrecognized class.');
      end
   end


function a = cosh(a)
%COSH         Gradient hyperbolic cosine cosh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  ax = sinh(a.x(:));
  a.x = cosh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    % take care of scalar a.x: cures Matlab V6.0 bug
    % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
    ax = ax(ia);
    if isa(a.x,'intval')
      adx = times(ax(:),sa(:),0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(:).*sa(:),sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


% Fit a piece-wise linear regression model.
% Here is the model
%
%  X \
%  | |
%  Q |
%  | /
%  Y
%
% where all arcs point down.
% We condition everything on X, so X is a root node. Q is a softmax, and Y is a linear Gaussian.
% Q is hidden, X and Y are observed.

X = 1;
Q = 2;
Y = 3;
dag = zeros(3,3);
dag(X,[Q Y]) = 1;
dag(Q,Y) = 1;
ns = [1 2 1]; % make X and Y scalars, and have 2 experts
dnodes = [2];
onodes = [1 3];
bnet = mk_bnet(dag, ns, 'discrete', dnodes, 'observed', onodes);

IRLS_iter = 10;
clamped = 0;

bnet.CPD{1} = root_CPD(bnet, 1);

if 0
  % start with good initial params
  w = [-5 5];  % w(:,i) is the normal vector to the i'th decisions boundary
  b = [0 0];  % b(i) is the offset (bias) to the i'th decisions boundary

  mu = [0 0];
  sigma = 1;
  Sigma = repmat(sigma*eye(ns(Y)), [ns(Y) ns(Y) ns(Q)]);
  W = [-1 1];
  W2 = reshape(W, [ns(Y) ns(X) ns(Q)]);

  bnet.CPD{2} = softmax_CPD(bnet, 2, w, b,  clamped, IRLS_iter);
  bnet.CPD{3} = gaussian_CPD(bnet, 3, mu, Sigma, W2);
else
  % start with rnd initial params
  rand('state', 0);
  randn('state', 0);
  bnet.CPD{2} = softmax_CPD(bnet, 2, 'clamped', clamped, 'max_iter', IRLS_iter);
  bnet.CPD{3} = gaussian_CPD(bnet, 3);
end

load('/examples/static/Misc/mixexp_data.txt', '-ascii');
% Just use 1/10th of the data, to speed things up
data = mixexp_data(1:10:end, :);
%data = mixexp_data;

%plot(data(:,1), data(:,2), '.')

s = struct(bnet.CPD{2}); % violate object privacy
%eta0 = [s.glim.b1; s.glim.w1]';
eta0 = [s.glim{1}.b1; s.glim{1}.w1]';
s = struct(bnet.CPD{3}); % violate object privacy
W = reshape(s.weights, [1 2]);
theta0 = [s.mean; W]';

%figure(1)
%mixexp_plot(theta0, eta0, data);
%suptitle('before learning')

ncases = size(data, 1);
cases = cell(3, ncases);
cases([1 3], :) = num2cell(data');

engine = jtree_inf_engine(bnet);

% log lik before learning
ll = 0;
for l=1:ncases
  ev = cases(:,l);
  [engine, loglik] = enter_evidence(engine, ev);
  ll = ll + loglik;
end

% do learning
max_iter = 5;
[bnet2, LL2] = learn_params_em(engine, cases, max_iter);

s = struct(bnet2.CPD{2});
%eta2 = [s.glim.b1; s.glim.w1]';
eta2 = [s.glim{1}.b1; s.glim{1}.w1]';
s = struct(bnet2.CPD{3});
W = reshape(s.weights, [1 2]);
theta2 = [s.mean; W]';

%figure(2)
%mixexp_plot(theta2, eta2, data);
%suptitle('after learning')

fprintf('mixexp2: loglik before learning %f, after %d iters %f\n', ll, length(LL2),  LL2(end));


function conv2pool()
    global config mem;
    curr_layer_idx = config.misc.current_layer;
    mem.layer_inputs{curr_layer_idx} = reshape(accumarray(mem.pooling_matrix{curr_layer_idx}, mem.activations{curr_layer_idx-1}(:), ...
                        [size(mem.activations{curr_layer_idx-1},1)*size(mem.activations{curr_layer_idx-1},2)/4, 1]), size(mem.activations{curr_layer_idx-1}, 2)/4, size(mem.activations{curr_layer_idx-1}, 1));
end


function C = addlongerror(C,err,exponent)
%ADDLONGERROR Add error to long number
%
%  C = addlongerror(A,err,exponent)
%
%Error term of C is updated by err. Third argument optional, default 0.
%If exponent is specified, error term is updated by err*10^exponent
%
%Input C, err and exponent may be column vectors
%

% written  12/30/98     S.M. Rump
% modfied  02/09/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 01/29/10     S.M. Rump  only warning if no interval arithmetic
% modified 08/26/12     S.M. Rump  global variables removed
%

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');

  if ~INTLAB_LONG_ERROR
    warning('long arithmetic changed to interval arithmetic, see longinit')
    INTLAB_LONG_ERROR = 1;
  end

  if any( err<0 )
    error('negative error specified in addlongerror')
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if nargin==3
    setround(1)
    % 10^exponent = beta^(Ernd+Efrac)
    E = exponent / log10(INTLAB_LONG_BETA);
    Ernd = round(E);
    Efrac = E - Ernd;
    err = err .* INTLAB_LONG_BETA.^Efrac;
    setround(0)                         % set rounding to nearest
  else
    Ernd = 0;
  end
  C.error = errorupdate( 1 , C.error , 0  , 1 , err , Ernd );
  C.error = errornormalize(C.error);

  C = normalize(C);

  setround(rndold)


function [y n] = var_nan(x)

for ii=1:size(x,2)
    tmp = x(:,ii);
    tmp(isnan(tmp))=[];
    y(ii)=var(tmp);
    n(ii)=size(tmp,1);
end


addpath('mex');

% we provide two sequences "car" and "table"
% example = 'table';
example = 'car';

% load the two frames
im1 = im2double(imread([example '1.jpg']));
im2 = im2double(imread([example '2.jpg']));

% im1 = imresize(im1,0.5,'bicubic');
% im2 = imresize(im2,0.5,'bicubic');

% set optical flow parameters (see Coarse2FineTwoFrames.m for the definition of the parameters)
alpha = 0.012;
ratio = 0.75;
minWidth = 20;
nOuterFPIterations = 7;
nInnerFPIterations = 1;
nSORIterations = 30;

para = [alpha,ratio,minWidth,nOuterFPIterations,nInnerFPIterations,nSORIterations];

% this is the core part of calling the mexed dll file for computing optical flow
% it also returns the time that is needed for two-frame estimation
tic;
[vx,vy,warpI2] = Coarse2FineTwoFrames(im1,im2,para);
toc

figure;imshow(im1);figure;imshow(warpI2);

% output gif
clear volume;
volume(:,:,:,1) = im1;
volume(:,:,:,2) = im2;
if exist('output','dir')~=7
    mkdir('output');
end
frame2gif(volume,fullfile('output',[example '_input.gif']));
volume(:,:,:,2) = warpI2;
frame2gif(volume,fullfile('output',[example '_warp.gif']));

% visualize flow field
clear flow;
flow(:,:,1) = vx;
flow(:,:,2) = vy;
imflow = flowToColor(flow);

figure;imshow(imflow);
imwrite(imflow,fullfile('output',[example '_flow.jpg']),'quality',100);


function x = sb_solve(sysmat,vecb)

% SB_SOLVE
%
% $Id$

%scalen
disp('Scaling stiffness matrix...')
dkond = 1./(sqrt(diag(sysmat)));
vecb = vecb.*dkond;
[indexi, indexj, s] = find(sysmat);
sys_size = size(sysmat,1);
clear sysmat;
s = (s.*dkond(indexi)).*dkond(indexj);
s(1) = 1;
disp('Preconditioning...')
L = sparse(indexi,indexj,s,sys_size,sys_size,length(s));
%partch
try
    L = ichol(L);
catch
    disp('Could not compute incomplete Cholesky-decompositon. Rescaling stiffness matrix...')
    alpha = 0.5d-6;
    alpha = alpha*8.d0;
    alpha = 1 / (alpha + 1);
    s = alpha*s;
    dia = find(indexi == indexj);
    s(dia) = (1./alpha)*s(dia);
    s(dia) = sqrt(s(dia));
    s(1) = 1;
    L = sparse(indexi,indexj,s,sys_size,sys_size,length(s));
    clear dia;
    L = ichol(L);
end
%startvektor
disp('Finding startvector...')
vecb_ = L \ (-vecb);
vecx = L' \ vecb_;
clear vecb_;
%sonstiges
sysmat = sparse(indexi,indexj,s,sys_size,sys_size,length(s));
sysmat = sysmat + sysmat' - sparse(1:sys_size,1:sys_size,diag(sysmat),sys_size,sys_size,sys_size);
clear indexi indexj s;
%dprod = sysmat * vecx;
%lösen
disp('Solving equation system...')
[x,fl,rr,it,rv]= pcg(sysmat,vecb,10e-9,5000,L,L',vecx);
%fl
rr
%it
%rescal
x = x.*dkond;
end


function z = s2z4(S);

% Z = s2z4(S)
%
% Scattering to Impedance transformation
% only for N-by-4 matrix

%d = (1 - S(:, 1)).*(1 - S(:, 4)) - S(:,2).* S(:, 3);

for i = 1:size(S,1)
  d(i) = (1 - S(i, 1)).*(1 - S(i, 4)) - S(i,2).* S(i, 3);
  while abs(d(i)) < 1e-8
    fckindex = 1+round(rand*3);
    S(i, fckindex) = S(i, fckindex)*(1+rand*1e-8);
    d(i) = (1 - S(i, 1)).*(1 - S(i, 4)) - S(i,2).* S(i, 3);
  end;
end;

d = rot90(d, 3);

% at this point the I-S matrix should be non-singular

z(:, 1) = ((1 + S(:, 1)).*(1 - S(:, 4)) + S(:,2).* S(:, 3))./d;
z(:, 2) = 2* S(:,2)./d;
z(:, 3) = 2* S(:,3)./d;
z(:, 4) = ((1 - S(:, 1)).*(1 + S(:, 4)) + S(:,2).* S(:, 3))./d;


c = 340;                    % Sound velocity (m/s)
fs = 16000;                 % Sample frequency (samples/s)
r = [2 1.5 3 ; 1 1.5 2];    % Receiver positions [x_1 y_1 z_1 ; x_2 y_2 z_2] (m)
s = [2 3.5 2];              % Source position [x y z] (m)
L = [5 4 6];                % Room dimensions [x y z] (m)
beta = 0.4;                 % Reverberation time (s)
n = 4096;                   % Number of samples
mtype = 'omnidirectional';  % Type of microphone
order = -1;                 % -1 equals maximum reflection order!
dim = 3;                    % Room dimension
orientation = 0;            % Microphone orientation (rad)
hp_filter = 1;              % Enable high-pass filter

h = rir_generator(c, fs, r, s, L, beta, n, mtype, order, dim, orientation, hp_filter);


function [doog_filters, texton_data] = cluster_learning_create_image_filters;

% create pair data
angle_step = 15;
filter_size = 5;
sigma = 1.5;
r = 0.25;

% create DOOG filters
num_angles=180/angle_step;
doog_filters = zeros(filter_size, filter_size, num_angles);
for (theta=0:angle_step:180-angle_step)
    doog_filters(:, :, theta/angle_step+1)=createDOOGFilter(sigma, r, theta, filter_size);
end

% load textons
if 0
texton_data = load('D:\Projects\Tools\textons\unitex_6_1_2_1.4_2_32.mat');
ordered_textons = select_diverse_textons(texton_data.tsim, 12);
new_tims = cell(length(ordered_textons), 1);
for i = 1:length(new_tims)
    new_tims{i} = texton_data.tim{ordered_textons(i)};
end
texton_data.tim = new_tims;
end

texton_data.tim = [];
%num_textons = length(texton_data.tim);


function [ output_data ] = deweightmem( input_data, weight_fun, oversample_rate, dim )
%DEWEIGHTMEM Make complex SAR uniformly weighted in one dimension
%
%    output_data = deweightmem(input_data, weight_fun, oversample_rate, dim)
%
%       Parameter name    Description
%
%       input_data        Array of complex values to deweight
%       weight_fun        Description of weighting applied.  Either a
%                            function handle to a function that takes a
%                            single argument (number of elements) and
%                            produces the weighting to apply, or a vector
%                            that is the weighting function sampled.
%       oversample_rate   Amount of sampling beyond the ImpRespBW in the
%                            processing dimension. (Default is Nyquist
%                            sampling = 1).
%       dim               Dimension over which to perform deweighting.
%                            Default is 1.
%       output_data       INPUT_DATA with normalization applied
%
% This implementation assumes that the data has already been "deskewed" and
% that the frequency support is centered.
%
% Author: Wade Schwartzkopf, NGA/R
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if ~exist('oversample_rate','var')
    oversample_rate = 1;
end
if ~exist('dim','var')
    dim = 1;
end

data_size = size(input_data,dim);
weight_size = round(data_size/oversample_rate); % Weighting only valid across ImpRespBW
if ~exist('weight_fun','var')||isempty(weight_fun) % No weighting passed in.  Do nothing.
    output_data = input_data;
    return;
elseif isa(weight_fun, 'function_handle')
    weighting = weight_fun(weight_size);
elseif isvector(weight_fun)
    weighting = interpft(weight_fun,weight_size);
end
% weighting = weighting * sqrt(mean(1./weighting.^2)); % We want total power maintained
weight_zp = ones(data_size,1); % Don't scale outside of ImpRespBW
weight_zp(floor((data_size-weight_size)/2)+(1:weight_size)) = weighting;
if dim==2
    weight_zp = weight_zp.';
end

% Divide out weighting in spatial frequency domain
output_data = fftshift(fft(input_data,[],dim),dim);
output_data = bsxfun(@rdivide,output_data,weight_zp);
output_data = ifft(ifftshift(output_data,dim),[],dim);

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


%% brute_force_tune
% Code to test the performance of various tuning parameters
% Works sorta like RANSAC I guess?
% Adam Werries 2016, see Apache 2.0 license.

k_max = 40;
% Specify ranges
init_b_a_unc = linspace(10,3000,100);
init_b_g_unc = linspace(0,500,100);
% Repeat arrays
init_b_a_unc = repmat(init_b_a_unc, [1 k_max]);
init_b_g_unc = repmat(init_b_g_unc, [1 k_max]);
% Generate random selections of each vector
num_items = length(init_b_a_unc);
ba_i = randperm(num_items);
bg_i = randperm(num_items);
rms_error_filter = zeros(1,num_items);
max_error_filter = zeros(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, aBiasUnc: %08.3f, gBiasUnc: %08.3f\n', i, init_b_a_unc(ba_i(i)), init_b_g_unc(bg_i(i)));
    temp_conf = LC_KF_config;
    temp_conf.init_b_a_unc = init_b_a_unc(ba_i(i)) * mug_to_mps2;
    temp_conf.init_b_g_unc = init_b_g_unc(bg_i(i)) * deg_to_rad / 3600;
    [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
    xyz = out_profile(:,2:4);
    llh = ecef2lla(xyz);
    [x,y] = deg2utm(llh(:,1),llh(:,2));
    x = x-min_x;
    y = y-min_y;

    distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
    rms_error_filter(i) = rms(distance);
    max_error_filter(i) = max(distance);
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.3f, rms is %08.3f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, aBiasUnc: %08.3f, gBiasUnc: %08.3f\n', i, init_b_a_unc(ba_i(i)), init_b_g_unc(bg_i(i)));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.3f, max is %08.3f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, aBiasUnc: %08.3f, gBiasUnc: %08.3f\n', i, init_b_a_unc(ba_i(i)), init_b_g_unc(bg_i(i)));


function pass = test_iszero( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end

% Test with function 1-1
f = ballfun(ones(21,20,22));
g = f-f;
pass(1) = iszero(g);

% Test with function 10^-20
f = ballfun(10^(-20));
pass(2) = (iszero(f) == 0);

% Test with function 0
f = ballfun(0);
pass(3) = iszero(f);
end


%% This file will be used to generate the simulation data of
% the Belousov Zhabotinsky reaction.
% Code By: K
% Last Updated: 2019/06/24
%% Clear all
close all;clear all;clc;
%% Add path
[fld_status, fld_msg, fld_msgID]=mkdir('Datas');
[fld_status, fld_msg, fld_msgID]=mkdir('Figures');
addpath('Datas')
addpath('Figures')
addpath('Functions')
%% Define parameters for the simulation
% Define diffusion rate
Dx=0.01;Dz=0.01;Ds=1;Du=1;

% Others
q=0.1;f=1.5;ksi=0.3;alpha=0.3;beta=0.26;gama=0.4;ksi2=1.5;ksi3=0.003;phi=0;

% Define the time horizon
dt=0.001;T=1;
tspan=0:dt:T;

% Define the spatial domain
L=20; % Total length of each dimension
n=128; % Discretization point of each dimension
N=n*n; % Total points used, n^2

x2=linspace(-L/2,L/2,n+1); x=x2(1:n);
y=x;dx=x(2)-x(1);

kx=(2*pi/L)*[0:(n/2-1) -n/2:-1];
ky=kx;

% Get n-dimensional grid
[X,Y]=meshgrid(x,y);
[KX,KY]=meshgrid(kx,ky);
K2=KX.^2+KY.^2; K22=reshape(K2,N,1);
Kx=reshape(KX,N,1);Ky=reshape(KY,N,1);Kxx=reshape(KX,N,1);Kyy=reshape(KY,N,1);

% Create a time matrix
r_t=zeros(n,n,length(tspan));r=zeros(n,n,length(tspan));r_x=zeros(n,n,length(tspan));r_xx=zeros(n,n,length(tspan));r_y=zeros(n,n,length(tspan));r_yy=zeros(n,n,length(tspan));
z_t=zeros(n,n,length(tspan));z=zeros(n,n,length(tspan));z_x=zeros(n,n,length(tspan));z_xx=zeros(n,n,length(tspan));z_y=zeros(n,n,length(tspan));z_yy=zeros(n,n,length(tspan));
s_t=zeros(n,n,length(tspan));s=zeros(n,n,length(tspan));s_x=zeros(n,n,length(tspan));s_xx=zeros(n,n,length(tspan));s_y=zeros(n,n,length(tspan));s_yy=zeros(n,n,length(tspan));
u_t=zeros(n,n,length(tspan));u=zeros(n,n,length(tspan));u_x=zeros(n,n,length(tspan));u_xx=zeros(n,n,length(tspan));u_y=zeros(n,n,length(tspan));u_yy=zeros(n,n,length(tspan));
r_lap=zeros(n,n,length(tspan));z_lap=zeros(n,n,length(tspan));s_lap=zeros(n,n,length(tspan));u_lap=zeros(n,n,length(tspan));
%% Set a Guassian for the initial condition
r0=cos(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.05,0.05,0,0)+GaussianFilter(X,Y,1,0.01,0,0)+GaussianFilter(X,Y,0.01,1,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.1;
z0=sin(0.1*sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.05,0.01,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.1;
s0=sin(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.5,0.5,0,5)+GaussianFilter(X,Y,0.5,0.5,5,0)+GaussianFilter(X,Y,0.5,0.5,0,-5)+GaussianFilter(X,Y,0.5,0.5,-5,0)+0.1;
u0=cos(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.5,0.01,0,0)+GaussianFilter(X,Y,0.01,0.5,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.01;

%% Plot the initial conditions
figure(1)
surf(x,y,r0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(2)
surf(x,y,z0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(3)
surf(x,y,s0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(4)
surf(x,y,u0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')
%%
close all
% Transfer the initial guess to fourier domain
xzsu0t=[reshape(fft2(r0),1,N) reshape(fft2(z0),1,N) reshape(fft2(s0),1,N) reshape(fft2(u0),1,N)].';

tic
opts = odeset('RelTol',1e-12,'AbsTol',1e-13);
NeedDev=0;
[time,xzsusol]=ode45(@(time,xzsut)BZ_Reaction_PDE(time,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev),tspan,xzsu0t,opts);
toc

%% Extract values
% After simulation, we extract the derivative
NeedDev=1;

for pin=1:length(tspan)
    % Get the derivative data
    [~,r_t(:,:,pin),z_t(:,:,pin),s_t(:,:,pin),u_t(:,:,pin),r(:,:,pin),z(:,:,pin),s(:,:,pin),u(:,:,pin),r_x(:,:,pin),z_x(:,:,pin),s_x(:,:,pin),u_x(:,:,pin),r_y(:,:,pin),z_y(:,:,pin),s_y(:,:,pin),u_y(:,:,pin),r_xx(:,:,pin),z_xx(:,:,pin),s_xx(:,:,pin),u_xx(:,:,pin),r_yy(:,:,pin),z_yy(:,:,pin),s_yy(:,:,pin),u_yy(:,:,pin),r_lap(:,:,pin),z_lap(:,:,pin),s_lap(:,:,pin),u_lap(:,:,pin)]=...
        BZ_Reaction_PDE(0,xzsusol(pin,:).',Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev);
end
%% Test out whehter the derivative is correct here
for pin=2:length(tspan)
    % Test out the zt (All time derivative tested, the Fourier derivative result matches the finite differences result)
    %     u_t_dum(:,:,pin)=(u(:,:,pin)-u(:,:,pin-1))/dt;
    %     figure(1)
    %     clf
    %     surf(x,y,u_t_dum(:,:,pin))
    %
    %     figure(2)
    %     clf
    %     surf(x,y,u_t(:,:,pin))
    %
    %     Dum=u_t_dum(:,:,pin)-u_t(:,:,pin);
    %     norm(Dum(:))

    % Test out the r_x, r_y (All tested, the answer matches)
    %     figure(1)
    %     clf
    %     surf(x,y,u_x(:,:,pin))
    %
    %     for kk=1:n
    %        Dum(kk,:)=CalDerivative(u(kk,:,pin),dx,1);
    %     end
    %
    %     figure(2)
    %     clf
    %     surf(x,y,Dum)
    %
    %     figure(3)
    %     clf
    %     surf(x,y,u_y(:,:,pin))
    %
    %     for kk=1:n
    %        Dum(:,kk)=CalDerivative(u(:,kk,pin),dx,1);
    %     end
    %
    %     figure(4)
    %     clf
    %     surf(x,y,Dum)

    % Test r_xx (All tested, the results matches)
    %     figure(1)
    %     clf
    %     surf(x,y,u_xx(:,:,pin))
    %
    %     for kk=1:n
    %         Dum_x(kk,:)=CalDerivative(u(kk,:,pin),dx,1);
    %     end
    %
    %     for kk=1:n
    %         Dum_xx(kk,:)=CalDerivative(Dum_x(kk,:),dx,1);
    %     end
    %
    %     figure(2)
    %     clf
    %     surf(x,y,Dum_xx)
    %
    %     figure(3)
    %     clf
    %     surf(x,y,u_yy(:,:,pin))
    %
    %     for kk=1:n
    %         Dum_y(:,kk)=CalDerivative(u(:,kk,pin),dx,1);
    %     end
    %
    %     for kk=1:n
    %         Dum_yy(:,kk)=CalDerivative(Dum_y(:,kk),dx,1);
    %     end
    %
    %     figure(4)
    %     clf
    %     surf(x,y,Dum_yy)
    %
    %     figure(5)
    %     clf
    %     surf(x,y,u_lap(:,:,pin))
    %
    %     figure(6)
    %     clf
    %     surf(x,y,Dum_xx+Dum_yy)
    %
    %     figure(7)
    %     clf
    %     surf(x,y,u_xx(:,:,pin)+u_yy(:,:,pin))
end

%% Plot the result and save it to file
%Creat Movie
Save_Animation=1;

States=['r','z','s','u'];
for pin=1:4
    if pin==1
        Animate=r;
        limits=[0 1];
    elseif pin==2
        Animate=z;
        limits=[0 1];
    elseif pin==3
        Animate=s;
        limits=[0 1];
    elseif pin==4
        Animate=u;
        limits=[0 1];
    end
    if Save_Animation==1
        name=strcat('BZ_Diffusion_',States(pin),'_',num2str(T),'_Seconds');
        writerObj = VideoWriter(name,'MPEG-4'); % Name it.
        writerObj.FrameRate = 20; % How many frames per second.
        writerObj.Quality = 100;
        open(writerObj);
        for j=1:15:length(tspan)
            clf
            pcolor(x,y,Animate(:,:,j))
            shading interp
            colormap(parula)
            caxis(limits)
            colorbar
            %Make plot propotional
            set(gca,'FontSize',18);
            set(gcf,'Position',[50 50 1000 1000]);
            set(gcf,'PaperPositionMode','auto');
            set(gca,'DataAspectRatio',[1 1 1])
            drawnow limitrate
            Mov=getframe(gcf);
            writeVideo(writerObj,Mov);
        end
        % Close the object or you can't read the final file correctly
        close(writerObj)
    end
end

%% Save files
Save_File=1;
if Save_File==1
    save(strcat('Datas\Simulation_BZ_',num2str(T),'_Seconds_',num2str(n),'_Grid.mat'),'r_t','z_t','s_t','u_t','r','z','s','u',...
        'r_x','z_x','s_x','u_x','r_y','z_y','s_y','u_y','r_xx','z_xx','s_xx','u_xx',...
        'r_yy','z_yy','s_yy','u_yy','dt','T','tspan','dx','x')
end


% SP_H1_ERROR: Evaluate the error in H^1 norm.
%
%   [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)
%
% INPUT:
%
%    space:   object defining the space of discrete functions (see sp_vector)
%    msh:     object defining the domain partition and the quadrature rule (see msh_cartesian)
%    u:       vector of dof weights
%    uex:     function handle to evaluate the exact solution
%    graduex: function handle to evaluate the gradient of the exact solution
%
% OUTPUT:
%
%     errh1:  error in H^1 norm
%     errl2:  error in L^2 norm
%     errh1s: error in H^1 seminorm
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
% 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)

  if (numel(u) ~= space.ndof)
    error ('Wrong size of the vector of degrees of freedom')
  end

  errl2 = 0; errh1s = 0;
  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (space, msh_col, 'value', true, 'gradient', true);

    [~, err_l2, err_h1s] = sp_h1_error (sp_col, msh_col, u, uex, graduex);

    errh1s = errh1s + err_h1s.^2;
    errl2 = errl2 + err_l2.^2;
  end

  errh1 = sqrt (errl2 + errh1s);
  errl2 = sqrt (errl2);
  errh1s = sqrt (errh1s);

end


function test_example_conditional_granger

% MEM 4gb
% WALLTIME 00:10:00

%
%% Conditional Granger causality in the frequency domain
%
% Conditional Granger causality is a derivative of spectral Granger causality that is computed over a triplet of channels (or blocks of channels). It provides the advantage that for this triplet, it allows to differentiate between a delayed parallel drive from sources A to be B and C and a sequential drive from A to B to C.
%
% This example illustrates the simulation and base analysis of the paper
%
% See also: [the connectivity tutorial]/tutorial/connectivity/).
%
%% # Setup and simulating the data sets
%
% First, define parameters under which samples should be simulated.
%
simcfg             = [];
simcfg.ntrials     = 500;
simcfg.triallength = 1;
simcfg.fsample     = 200;
simcfg.nsignal     = 3;
simcfg.method      = 'ar';

% We want to simulate a system with three signals. Their noise is modeled as white noise processes with zero mean and standard deviations
%
%
% They will have the covariances ζ, η and ε. We also require a paramters μ=0.5.
%
% parameters of the model itself
mu                 = 0.5;
absnoise           = [ 1.0   0.2   0.3 ];

% First, we generate the sample for the case of sequential driving.
% We want to incorporate the system
%
%
% which we can do like this:
%
% params(i,j,k): j -> i at t=k
simcfg.params(:,:,1) = [   0      0      0;
                         1.0      0      0;
                           0    1.0     mu];

% Note that the matrix representation for the covariance reads from columns to row, other than the MVAR-model is read intuitively. But we still need to hand the parameters of the noise to the model:
%
% paper defines stds, not cov:
simcfg.noisecov      = diag(absnoise.^2);

data2           = ft_connectivitysimulation(simcfg);

% Now create sample data for the case of differentially delayed driving,
%
%
% which we can write as
%
simcfg.params(:,:,1) = [   0      0      0;
                         1.0      0      0;
                           0      0     mu];
simcfg.params(:,:,2) = [   0      0      0;
                           0      0      0;
                         1.0      0      0];

% We build the actual MVAR-representation...
%
data1           = ft_connectivitysimulation(simcfg);

% #
%
figure
plot(data1.time{1}, data1.trial{1})
legend(data1.label)
xlabel('time (s)')

% Don't be confused that we started with data2 and conclude with data1. This is just to maintain the order the systems have in the paper.
%
%% # MVAR model frequency analysis
% We generate spectral representations from the MVAR representations we defined with data1 and data2. After all, we want to compute spectral Granger causality. Fast Fourier is a good starting point.
%
freq                   = [];
freq.freqcfg           = [];
freq.freqcfg.method    = 'mtmfft';
freq.freqcfg.output    = 'fourier';
freq.freqcfg.tapsmofrq = 2;
freqdata1           = ft_freqanalysis(freq.freqcfg, data1);
freqdata2           = ft_freqanalysis(freq.freqcfg, data2);

%% # "Regular" Granger causality
% Let first compute regular bivariate Granger causality, as this makes the difference clear to what we want.
%
grangercfg = [];
grangercfg.method  = 'granger';
grangercfg.granger.conditional = 'no';
grangercfg.granger.sfmethod = 'bivariate';

gdata = [];
gdata.g1_bivar_reg      = ft_connectivityanalysis(grangercfg, freqdata1);
gdata.g2_bivar_reg      = ft_connectivityanalysis(grangercfg, freqdata2);

%% # Multivariate conditional Granger causality
% However, we clearly want a multivariate approach. Also, we need to define channel combinations, as we now require triplets of inputs.
%
grangercfg.granger.conditional = 'yes';
grangercfg.channelcmb  = {'signal001', 'signal002', 'signal003'};
grangercfg.granger.sfmethod = 'multivariate';
grangercfg.granger.conditional = 'yes';

% block-wise causality
grangercfg.granger.block(1).name   = freqdata1.label{1};
grangercfg.granger.block(1).label  = freqdata1.label(1);
grangercfg.granger.block(2).name   = freqdata1.label{2};
grangercfg.granger.block(2).label  = freqdata1.label(2);
grangercfg.granger.block(3).name   = freqdata1.label{3};
grangercfg.granger.block(3).label  = freqdata1.label(3);

gdata.g1_multi_reg_conditional = ft_connectivityanalysis(grangercfg, freqdata1);
gdata.g2_multi_reg_conditional = ft_connectivityanalysis(grangercfg, freqdata2);

%% # Evaluation
% The label combinations are 6x2 cell arrays, containing all 2-permutations
% tuplets from the channels. How to interpret this?
% Is the combination a, b representing Fa→b|c?
% Let's check this. In scenario 2, we should clearly see a higher causality
% from 1→3 | 2 than in scenario 1 of the differentially delayed
% drive. This corresponds to row 4 in the
% gdata.g1_multi_reg_conditional.labelcmb.
% So, let's compare the labelcmb 1, 3 in both scenarios:
%
scenario1_mean = mean(gdata.g1_multi_reg_conditional.grangerspctrm(4, :));
scenario2_mean = mean(gdata.g2_multi_reg_conditional.grangerspctrm(4, :));


function knee_angle(nlp, bounds)
    % constraints for step length and step width

    domain = nlp.Plant;
    x = domain.States.x;

    % knee angle
    knee = [x('qBRight') - x('qARight')
        x('qBLeft') - x('qALeft')];
    knee_fun = SymFunction(['kneeAngles_',domain.Name], knee, {x});
    addNodeConstraint(nlp, knee_fun, {'x'}, 'all', ...
        bounds.constrBounds.knee.lb, ...
        bounds.constrBounds.knee.ub,'Linear');

end