function [fx,tx,pv,fv]=fxpefac(s,fs,tinc,m,pp)
%FXPEFAC PEFAC pitch tracker [FX,TT,PV,FV]=(S,FS,TINC,M,PP)
%
% Input:   s(ns)      Speech signal
%          fs         Sample frequency (Hz)
%          tinc       Time increment between frames (s) [0.01]
%                     or [start increment end]
%          m          mode
%                     'g' plot graph showing waveform and pitch
%                     'G' plot spectrogram with superimposed pitch using
%                         options pp.sopt [default: 'ilcwpf']
%                     'x' use external files for algorithm parameter
%                         initialization: fxpefac_g and fxpefac_w
%          pp         structure containing algorithm parameters
%
% Outputs: fx(nframe)     Estimated pitch (Hz)
%          tx(nframe)     Time at the centre of each frame (seconds).
%          pv(nframe)     Probability of the frame of being voiced
%          fv             structure containing feature vectors
%                           fv.vuvfea(nframe,2) = voiced/unvoiced GMM features

% References
%  [1]  S.Gonzalez and M. Brookes,
%       A pitch estimation filter robust to high levels of noise (PEFAC), Proc EUSIPCO,Aug 2011.

% Bugs/Suggestions
% (1) do long files in chunks
% (2) option of n-best DP

%	   Copyright (C) Sira Gonzalez and Mike Brookes 2011
%      Version: $Id: fxpefac.m 3601 2013-10-11 15:27:30Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent w_u m_u v_u w_v m_v v_v dpwtdef
% initialize persistent variables
if ~numel(w_u)

    % voiced/unvoiced decision based on 2-element feature vector
    % (a) mean power of the frame's log-freq spectrum (normalized so its short-term average is LTASS)
    % (b) sum of the power in the first three peaks
    %===== VUV
    if nargin>3 && any(m=='x')
        fxpefac_g;     % read in GMM parameters
        fxpefac_w;     % read in Weights parameters
    else
        w_u=[0.1461799 0.3269458 0.2632178 0.02331986 0.06360947 0.1767271 ]';

        m_u=[13.38533 0.4199435 ;
             12.23505 0.1496836 ;
             12.76646 0.2581733 ;
             13.69822 0.6893078 ;
             9.804372 0.02786567 ;
             11.03848 0.07711229 ];

        v_u=reshape([0.4575519 0.002619074 0.002619074 0.01262138 ;
             0.7547719 0.008568089 0.008568089 0.001933864 ;
             0.5770533 0.003561592 0.003561592 0.00527957 ;
             0.3576287 0.01388739 0.01388739 0.04742106 ;
             0.9049906 0.01033191 0.01033191 0.0001887114 ;
             0.637969 0.009936445 0.009936445 0.0007082946 ]',[2 2 6]);

        w_v=[0.1391365 0.221577 0.2214025 0.1375109 0.1995124 0.08086066 ]';

        m_v=[15.36667 0.8961554 ;
             13.52718 0.4809653 ;
             13.95531 0.8901121 ;
             14.56318 0.6767258 ;
             14.59449 1.190709 ;
             13.11096 0.2861982 ];

        v_v=reshape([0.196497 -0.002605404 -0.002605404 0.05495016 ;
             0.6054919 0.007776652 0.007776652 0.01899244 ;
             0.5944617 0.0485788 0.0485788 0.03511229 ;
             0.3871268 0.0292966 0.0292966 0.02046839 ;
             0.3377683 0.02839657 0.02839657 0.04756354 ;
             1.00439 0.03595795 0.03595795 0.006737475 ]',[2 2 6]);
    end
    %===== PDP
    %     dfm = -0.4238; % df mean
    %     dfv = 3.8968; % df variance (although treated as std dev here)
    %     delta = 0.15;
    %     dflpso=[dfm 0.5/(log(10)*dfv^2) -log(2*delta/(dfv*sqrt(2*pi)))/log(10)]; % scale factor & offset for df pdf
    %     dpwtdef=[1.0000, 0.8250, 1.3064, 1.9863]; % default DP weights
    dpwtdef=[1.0000, 0.8250, 0.01868, 0.006773, 98.9, -0.4238]; % default DP weights
    %===== END

end
% Algorithm parameter defaults

p.fstep=5;              % frequency resolution of initial spectrogram (Hz)
p.fmax=4000;            % maximum frequency of initial spectrogram (Hz)
p.fres = 20;            % bandwidth of initial spectrogram (Hz)
p.fbanklo = 10;         % low frequency limit of log filterbank (Hz)
p.mpsmooth = 21;       % width of smoothing filter for mean power
% p.maxtranf = 1000;      % maximum value of tranf cost term
p.shortut = 7;          % max utterance length to average power of entire utterance
p.pefact = 1.8;         % shape factor in PEFAC filter
p.numopt = 3;           % number of possible frequencies per frame
p.flim = [60 400];      % range of feasible fundamental frequencies (Hz)
p.w = dpwtdef;          % DP weights
% p.rampk = 1.1;          % constant for relative-amplitude cost term
% p.rampcz = 100;         % relative amplitude cost for missing peak
p.tmf = 2;              % median frequency smoothing interval (s)
p.tinc = 0.01;          % default frame increment (s)
p.sopt = 'ilcwpf';      % spectrogram options

% update parameters from pp argument

if nargin>=5 && isstruct(pp)
    fnq=fieldnames(pp);
    for i=1:length(fnq)
        if isfield(p,fnq{i})
            p.(fnq{i})=pp.(fnq{i});
        end
    end
end

% Sort out input arguments
if nargin>=3  && numel(tinc)>0
    p.tinc = tinc;   % 0.01 s between consecutive time frames
end
if nargin<4
    m='';
end

% Spectrogram of the mixture
fmin = 0; fstep = p.fstep; fmax = p.fmax;
fres = p.fres;  % Frequency resolution (Hz)
[tx,f,MIX]=spgrambw(s,fs,fres,[fmin fstep fmax],[],p.tinc);
nframes=length(tx);
txinc=tx(2)-tx(1);  % actual frame increment
%  ==== we could combine spgrambw and filtbankm into a single call to spgrambw or use fft directly ====
% Log-frequency scale
[trans,cf]=filtbankm(length(f),2*length(f)-1,2*f(end),p.fbanklo,f(end),'usl');
O = MIX*trans'; % Original spectrum in Log-frequency scale

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Amplitude Compression

% Calculate alpha based on LTASS ratios
ltass = stdspectrum(6,'p',cf);
auxf = [cf(1),(cf(1:end-1)+cf(2:end))./2,cf(end)];
ltass = ltass.*diff(auxf);                  % weight by bin width

% estimated ltass
O = O.*repmat(diff(auxf),nframes,1);     % weight spectrum by bin width
O1 = O;

if tx(end)<p.shortut                        % if it is a short utterance
    eltass = mean(O,1);                     % mean power per each frequency band
    eltass = smooth(eltass,p.mpsmooth);     % smooth in log frequency
    eltass= eltass(:).';                    % force a row vector

    % Linear AC
    alpha = (ltass)./(eltass);
    alpha = alpha(:).';
    alpha = repmat(alpha,nframes,1);
    O = O.*alpha;                           % force O to have an average LTASS spectrum

    % ==== should perhaps exclude the silent portions ***
else                                        % long utterance

    tsmo = 3; % time smoothing over 3 sec
    stt = round(tsmo/txinc);
    eltass = timesm(O,stt);
    eltass = smooth(eltass,p.mpsmooth);     % filter in time and log frequency

    % Linear AC
    alpha = repmat(ltass,nframes,1)./(eltass);
    O = O.*alpha;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create the filter to detect the harmonics
ini = find(cf>3*cf(1));
sca = cf/cf(ini(1)); % bin frequencies start at approximately 0.33 with sca(ini(1))=1 exactly

% Middle
sca = sca(sca<10.5 & sca>0.5);  % restrict to 0.5 - 10.5 times fundamental

sca1 = sca;
filh = 1./(p.pefact-cos(2*pi*sca1));
filh = filh - sum(filh(1:end).*diff([sca1(1),(sca1(1:end-1)+sca1(2:end))./2,sca1(end)]))/sum(diff([sca1(1),(sca1(1:end-1)+sca1(2:end))./2,sca1(end)]));

posit = find(sca>=1);  % ==== this should just equal ini(1) ====
negat = find(sca<1);
numz = length(posit)-1-length(negat);
filh = filh./max(filh);
filh = [zeros(1,numz) filh]; % length is always odd with central value = 1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filter the log-frequency scaled spectrogram
B = imfilter(O,filh);  % does a convolution with zero lag at centre of filh

% Feasible frequency range
numopt = p.numopt; % Number of possible fundamental frequencies per frame
flim = p.flim;
pfreq = find(cf>flim(1) & cf<flim(2)); % flim = permitted fx range = [60 400]
ff = zeros(nframes,numopt);
amp = zeros(nframes,numopt);
for i=1:nframes
    [pos,peak]=v_findpeaks(B(i,pfreq),[],5/(cf(pfreq(2))-cf(pfreq(1)))); % min separation = 5Hz @ fx=flim(1) (could pre-calculate) ====
    if numel(pos)
        [peak,ind]=sort(peak,'descend');
        pos = pos(ind);                     % indices of peaks in the B array
        posff = cf(pfreq(pos));             % frequencies of peaks
        fin = min(numopt,length(posff));
        ff(i,1:fin)=posff(1:fin);           % save both frequency and amplitudes
        amp(i,1:fin)=peak(1:fin);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Probabilitly of the frame of being voiced

% voiced/unvoiced decision based on 2-element feature vector
% (a) mean power of the frame's log-freq spectrum (normalized so its short-term average is LTASS)
% (b) sum of the power in the first three peaks

pow = mean(O,2);

vuvfea = [log(pow) 1e-3*sum(amp,2)./(pow+1.75*1e5)];

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

pru=gaussmixp(vuvfea,m_u,v_u,w_u);  % Probability of being unvoiced
prv=gaussmixp(vuvfea,m_v,v_v,w_v);  % Probability of being voiced

pv=(1+exp(pru-prv)).^(-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Dynamic programming

% w(1): relative amp, voiced local cost
% w(2): median pitch deviation cost
% w(3): df cost weight
% w(4): max df cost
% w(5): relative amp cost for missing peaks (very high)
% w(6): df mean

w = p.w;

% Relative amplitude
camp = -amp./repmat(max(amp,[],2),1,numopt);  % relative amplitude used as cost
camp(amp==0)=w(5); % If no frequency found

% Time interval for the median frequency
tmf = p.tmf; % in sec
inmf = round(tmf/txinc);

%--------------------------------------------------------------------------
% FORWARDS
% Initialize values
cost = zeros(nframes,numopt);
prev = zeros(nframes,numopt);
medfx = zeros(nframes,1);
dffact=2/txinc;

% First time frame
% cost(1,:) = w(1)*ramp(1,:);
cost(1,:) = w(1)*camp(1,:);  % only one cost term for first frame
fpos = ff(1:min(inmf,end),1);
mf=median(fpos(pv(1:min(inmf,end))>0.6));   % calculate median frequency of first 2 seconds
if isnan(mf)
    mf=median(fpos(pv(1:min(inmf,end))>0.5));
    if isnan(mf)
        mf=median(fpos(pv(1:min(inmf,end))>0.4));
        if isnan(mf)
            mf=median(fpos(pv(1:min(inmf,end))>0.3)); % ==== clumsy way of ensuring that we take the best frames ====
            if isnan(mf)
                mf=0;
            end
        end
    end
end
medfx(1)=mf;

for i=2:nframes              % main dynamic programming loop
    if i>inmf
        fpos = ff(i-inmf:i,1);  % fpos is the highest peak in each frame
        mf=median(fpos(pv(1:inmf)>0.6));  % find median frequency over past 2 seconds
        if isnan(mf)
            mf=median(fpos(pv(1:inmf)>0.5));
            if isnan(mf)
                mf=median(fpos(pv(1:inmf)>0.4));
                if isnan(mf)
                    mf=median(fpos(pv(1:inmf)>0.3));% ==== clumsy way of ensuring that we take the best frames ====
                    if isnan(mf)
                        mf=0;
                    end
                end
            end
        end
    end
    medfx(i)=mf;
    % Frequency difference between candidates and cost
    df = dffact*(repmat(ff(i,:).',1,numopt) - repmat(ff(i-1,:),numopt,1))./(repmat(ff(i,:).',1,numopt) + repmat(ff(i-1,:),numopt,1));
    costdf=w(3)*min((df-w(6)).^2,w(4));

    % Cost related to the median pitch
    if mf==0                                   % this test was inverted in the original version
        costf = zeros(1,numopt);
    else
        costf = abs(ff(i,:) - mf)./mf;
    end
    [cost(i,:),prev(i,:)]=min(costdf + repmat(cost(i-1,:),numopt,1),[],2); % ==== should we allow the possibility of skipping frames ? ====
    cost(i,:)=cost(i,:)+w(2)*costf + w(1)*camp(i,:);  % add on costs that are independent of previous path

end

% Traceback

fx=zeros(nframes,1);
ax=zeros(nframes,1);
best = zeros(nframes,1);

nose=find(cost(end,:)==min(cost(end,:))); % ==== bad method (dangerous) ===
best(end)=nose(1);
fx(end)=ff(end,best(end));
ax(end)=amp(end,best(end));
for i=nframes:-1:2
    best(i-1)=prev(i,best(i));
    fx(i-1)=ff(i-1,best(i-1));
    ax(i-1)=amp(i-1,best(i-1));
end

if nargout>=4
    fv.vuvfea=vuvfea;  % voiced-unvoiced features
    fv.best=best;  % selected path
    fv.ff=ff;  % pitch candidates
    fv.amp=amp;  % pitch candidate amplitudes
    fv.medfx=medfx;  % median pitch
    fv.w=w;  % DP weights
    fv.dffact=dffact;  % df scale factor
    fv.hist = [log(mean(O,2)) sum(amp,2)./((mean(O,2)))];
end

if ~nargout || any(m=='g') || any(m=='G')
    nax=0;  % number of axes sets to link
    msk=pv>0.5; % find voiced frames as a mask
    fxg=fx;
    fxg(~msk)=NaN; % allow only good frames
    fxb=fx;
    fxb(msk)=NaN; % allow only bad frames
    if any(m=='G') || ~nargout && ~any(m=='g')
        clf;
        spgrambw(s,fs,p.sopt); % draw spectrogram with log axes
        hold on
        plot(tx,log10(fxg),'-b',tx,log10(fxb),'-r'); % fx track
        yy=get(gca,'ylim');
        plot(tx,yy(1)+yy*[-1;1]*(0.02+0.05*pv),'-k'); % P(V) track
        hold off
        nax=nax+1;
        axh(nax)=gca;
        if any(m=='g')
            figure;   % need a new figure if plotting two graphs
        end
    end
    if any(m=='g')
        ns=length(s);
        [tsr,ix]=sort([(1:ns)/fs 0.5*(tx(1:end-1)+tx(2:end))']); % intermingle speech and frame boundaries
        jx(ix)=1:length(ix); % create inverse index
        sp2fr=jx(1:ns)-(0:ns-1);  % speech sample to frame number
        spmsk=msk(sp2fr);   % speech sample voiced mask
        sg=s;
        sg(~spmsk)=NaN;   % good speech samples only
        sb=s;
        sb(spmsk)=NaN;    % bad speech samples only
        clf;
        subplot(5,1,1);
        plot(tx,pv,'-b',(1:ns)/fs,0.5*mod(cumsum(fx(sp2fr)/fs),1)-0.6,'-b');
        nax=nax+1;
        axh(nax)=gca;
        ylabel('\phi(t), P(V)');
        set(gca,'ylim',[-0.65 1.05]);
        subplot(5,1,2:3);
        plot((1:ns)/fs,sg,'-b',(1:ns)/fs,sb,'-r');
        nax=nax+1;
        axh(nax)=gca;
        subplot(5,1,4:5);
        plot(tx,fxg,'-b',tx,fxb,'-r');
        ylabel('Pitch (Hz)');
        %         semilogy(tx,fxg,'-b',tx,fxb,'-r');
        %         ylabel(['Pitch (' yticksi 'Hz)']);
        set(gca,'ylim',[min(fxg)-30 max(fxg)+30]);
        nax=nax+1;
        axh(nax)=gca;
    end
    if nax>1
        linkaxes(axh,'x');
    end
end

function y=smooth(x,n)
nx=size(x,2);
nf=size(x,1);
c=cumsum(x,2);
y=[c(:,1:2:n)./repmat(1:2:n,nf,1) (c(:,n+1:end)-c(:,1:end-n))/n (repmat(c(:,end),1,floor(n/2))-c(:,end-n+2:2:end-1))./repmat(n-2:-2:1,nf,1)];

function y=timesm(x,n)
if ~mod(n,2)
    n = n+1;
end
nx=size(x,2);
nf=size(x,1);
c=cumsum(x,1);
mid = round(n/2);
y=[c(mid:n,:)./repmat((mid:n).',1,nx); ...
    (c(n+1:end,:)-c(1:end-n,:))/n; ...
    (repmat(c(end,:),mid-1,1) - c(end-n+1:end-mid,:))./repmat((n-1:-1:mid).',1,nx)];


function FEP_fluctuations
% This demonstration  uses an ensemble of particles with intrinsic (Lorentz
% attractor) dynamics and (Newtonian) short-range coupling.  The focus of
% this routine is to unpack the Bayesian perspective. We first simulate
% dynamics  to nonequilibrium steady-state, identify the Markov blanket and
% then examine the encoding of external states by internal states; in terms
% of their expected values.
%
% The crucial aspect of this implicit inference (and the basis of the free
% energy principle) is the existence of a conditional synchronisation
% manifold, when conditioning internal and external states on the Markov
% blanket. This provides the basis for a mapping between internal and
% external states that can be interpreted in terms of a probabilistic
% representation or inference.
%
% This Bayesian perspective is illustrated in terms of a mapping between
% the canonical modes of internal and external states (as approximated
% with a polynomial expansion). The canonical modes her are evaluated
% using an estimate of the conditional expectations  based upon the
% Euclidean proximity of Markov blanket states. The ensuing posterior over
% external states is than illustrated, in relation to the actual external
% states. We also  simulate event related potentials by identifying
% several points in time when the Markov blankets revisit the same
% neighbourhood. Finally, to illustrate the underlying dynamics, the
% Jacobians or coupling among internal and external states are
% presented; using different orders of coupling (i.e., degrees of
% separation)
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: FEP_fluctuations.m 7163 2017-09-04 09:12:50Z karl $

% default settings (GRAPHICS sets movies)
%--------------------------------------------------------------------------
rng('default')
GRAPHICS = 1;

% Demo of synchronization manifold using coupled Lorenz attractors
%==========================================================================
N    = 128;                         % number of (Lorenz) oscillators
T    = 2048;                        % number of time bins
dt   = 1/32;                        % time interval

% parameters
%--------------------------------------------------------------------------
P.k  = 1 - exp(-rand(1,N)*4);       % variations in temporal scale
P.d  = 1/8;                         % amplitude of random fluctuations

% states
%--------------------------------------------------------------------------
x.p  = randn(2,N)*4;                % microstates (position)
x.v  = zeros(2,N);                  % microstates (velocity)
x.q  = randn(3,N)/32;               % microstates (states)
u    = zeros(1,T);                  % exogenous fluctuations

% generate an dynamics from initial conditions
%==========================================================================
spm_figure('GetWin','Markov blanket');clf
if GRAPHICS
    subplot(2,2,1)
else
    subplot(2,1,1)
end
[Q,X,V,A,x] = spm_soup(x,u,P,T,dt,1);

% States
%--------------------------------------------------------------------------
% Q    - history of microstates (states)
% X    - history of microstates (position)
% V    - history of microstates (velocity)

for i = 1:size(X,3)
    S(:,:,i) = [Q(:,:,i);X(:,:,i);V(:,:,i)];
end

% Markov blanket - parents, children, and parents of children
%==========================================================================

% Adjacency matrix
%--------------------------------------------------------------------------
t     = (T - 256):T;                              % final time indices
L     = sparse(double(any(A(:,:,t),3)))';

% internal states (defined by principle eigenvector of Markov blanket)
%--------------------------------------------------------------------------
B     = double((L + L' + L'*L));
B     = B - diag(diag(B));
v     = spm_svd(B*B',1);
[v,j] = sort(abs(v(:,1)),'descend');

% get Markov blanket and divide into sensory and active states
%--------------------------------------------------------------------------
m     = j(1:8);                                   % internal cluster
mm    = sparse(m,1,1,N,1);                        % internal states
bb    = B*mm & (1 - mm);                          % Markov blanket
ee    = 1 - bb - mm;                              % external states
b     = find(bb);
e     = find(ee);
m     = find(mm);
s     = b(find( any(L(b,e),2)));
a     = b(find(~any(L(b,e),2)));

% adjacency matrix - with partition underneath (LL)
%--------------------------------------------------------------------------
k       = [e; s; a; m];
LL      = L;
LL(e,e) = LL(e,e) + 1/8;
LL(s,s) = LL(s,s) + 1/8;
LL(a,a) = LL(a,a) + 1/8;
LL(m,m) = LL(m,m) + 1/8;
LL      = LL(k,k);

% plot dynamics for the initial and subsequent time periods
%--------------------------------------------------------------------------
subplot(4,1,3)
r    = 1:512;
plot(r,squeeze(Q(1,e,r)),':c'), hold on
plot(r,squeeze(Q(1,m,r)),' b'), hold off
axis([r(1) r(end) -32 32])
xlabel('Time','FontSize',12)
title('Electrochemical dynamics','FontSize',16)

subplot(4,1,4)
r    = 1:T;
plot(r,squeeze(V(1,e,r)),':c'), hold on
plot(r,squeeze(V(1,m,r)),' b'), hold off
axis([r(1) r(end) -32 32])
xlabel('Time','FontSize',12)
title('Newtonian dymanics','FontSize',16)

% Markov blanket - self-assembly
%==========================================================================
subplot(2,2,1)
imagesc(1 - LL)
axis square
xlabel('Element','FontSize',12)
xlabel('Element','FontSize',12)
title('Adjacency matrix','FontSize',16)

% follow self-assembly
%--------------------------------------------------------------------------
clear M
for i = (T - 512):T

    % plot positions
    %----------------------------------------------------------------------
    subplot(2,2,2),set(gca,'color','w')

    px = ones(3,1)*X(1,:,i) + Q([1 2 3],:,i)/16;
    py = ones(3,1)*X(2,:,i) + Q([2 3 1],:,i)/16;
    plot(px,py,'.b','MarkerSize',8), hold on
    px = X(1,e,i); py = X(2,e,i);
    plot(px,py,'.c','MarkerSize',24)
    px = X(1,m,i); py = X(2,m,i);
    plot(px,py,'.b','MarkerSize',24)
    px = X(1,s,i); py = X(2,s,i);
    plot(px,py,'.m','MarkerSize',24)
    px = X(1,a,i); py = X(2,a,i);
    plot(px,py,'.r','MarkerSize',24)

    xlabel('Position','FontSize',12)
    ylabel('Position','FontSize',12)
    title('Markov Blanket','FontSize',16)
    axis([-1 1 -1 1]*8)
    axis square, hold off, drawnow

    % save
    %----------------------------------------------------------------------
    if i > (T - 128) && GRAPHICS
        M(i - T + 128) = getframe(gca);
    end

end

% set ButtonDownFcn
%--------------------------------------------------------------------------
if GRAPHICS
    h   = findobj(gca);
    set(h(1),'Userdata',{M,16})
    set(h(1),'ButtonDownFcn','spm_DEM_ButtonDownFcn')
    xlabel('Click for Movie','Color','r')
end

% illustrate the Bayesian perspective (predictability of external states)
%==========================================================================
spm_figure('GetWin','Bayesian perspective');clf

% establish a statistical dependency between internal (dynamic) states (XQ)
%--------------------------------------------------------------------------
T     = 512;                                   % length of timeseries
t     = size(X,3) - T - 2;
for i = 1:T
    Xe(i,:) = spm_vec(V(:,e,i + t));           % external states
    Xb(i,:) = spm_vec(S(:,[a;s],i + t));       % Markov blanket
    Xm(i,:) = spm_vec(Q(:,m,i + t));           % internal states
end
xe    = zeros(size(Xe));
xm    = zeros(size(Xm));
iC    = inv(cov(Xb));

% probabilistic proximity in the space of the Markov blanket
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:T
        r      = Xb(i,:) - Xb(j,:);
        w(i,j) = exp(-(r*iC*r')/128);
    end
end

% convert into proper probability distribution
%--------------------------------------------------------------------------
w = diag(sum(w,2))\w;

% mean
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:T
        xe(i,:) = xe(i,:) + w(i,j)*Xe(j,:);
        xm(i,:) = xm(i,:) + w(i,j)*Xm(j,:);
    end
end

% covariance (not used)
%--------------------------------------------------------------------------
% ce    = zeros(size(Xe,1),size(Xe,2),size(Xe,2));
% for i = 1:T
%     for j = 1:T
%         ce(i,:,:) = squeeze(ce(i,:,:)) + w(i,j)*(Xe(i,:) - xe(i,:))'*(Xe(j,:) - xe(j,:));
%     end
% end

% normalise and identify canonical eigenvariates
%--------------------------------------------------------------------------
xe    = spm_detrend(xe);
xm    = spm_detrend(xm);
CVA   = spm_cva(xe,xm);

% show results - canonical vectors over elements (mode M)
%--------------------------------------------------------------------------
subplot(3,2,1)

M     = 1;
Ve    = CVA.V(:,M);
Ve    = spm_unvec(Ve,V(:,e,1));
ve    = sum(Ve.^2);
ve    = ve/max(ve);
for k = 1:length(Ve)
    c = [0 1 1]*ve(k) + [1 1 1]*(1 - ve(k));
    plot(X(1,e(k),end),X(2,e(k),end),'.','MarkerSize',32,'Color',c), hold on
end

% overplot mode of motion
%--------------------------------------------------------------------------
quiver(X(1,e,end),X(2,e,end),Ve(1,:),Ve(2,:))

Vm    = CVA.W(:,M);
Vm    = spm_unvec(Vm,Q(:,m,1));
vm    = sum(Vm.^2);
vm    = vm/max(vm);
for k = 1:length(Vm)
    c = [0 0 1]*vm(k) + [1 1 1]*(1 - vm(k));
    plot(X(1,m(k),end),X(2,m(k),end),'.','MarkerSize',32,'Color',c), hold on
end
xlabel('Position', 'FontSize',12)
ylabel('Position','FontSize',12)
title('Canonical mode','FontSize',16)

% conditional synchronisation manifold (polynomial approximation)
%==========================================================================

% polynomial approximation
%--------------------------------------------------------------------------
xX    = xm*CVA.W(:,M);
XX    = [xX.^0 xX.^1 xX.^2 xX.^3 xX.^4 xX.^5];
bE    = pinv(XX)*Xe*CVA.V(:,M);
qE    = XX*bE;

% conditional expectation and variance
%--------------------------------------------------------------------------
% for i = 1:T
%     qC(i) = CVA.V(:,1)'*squeeze(ce(i,:,:))*CVA.V(:,1);
% end
qC    = ones(size(qE));
qC    = abs(var(Xe*CVA.V(:,1) - qE)*qC/mean(qC));

% show results - conditional synchronisation manifold
%--------------------------------------------------------------------------
subplot(3,2,2)
plot(CVA.w(:,1),Xe*CVA.V(:,1),'.c' ), hold on
plot(CVA.w(:,1),qE,'.b' ), hold off
xlabel('Internal mode', 'FontSize',12)
ylabel('External mode','FontSize',12)
title('Synchronisation manifold','FontSize',16), spm_axis tight

% show results - conditional distributions as a function of time
%--------------------------------------------------------------------------
subplot(3,1,2)
plot(Xe*CVA.V(:,1),'c' ), hold on
spm_plot_ci(qE',qC(:)'),  hold off
xlabel('Time', 'FontSize',12)
ylabel('External states','FontSize',12)
title('Inferred and real motion','FontSize',16), spm_axis tight

%  event related potentials
%==========================================================================

%  identify points of interest using the external canonical variate
%--------------------------------------------------------------------------
u     = CVA.v(:,1);
ue    = [];
for i = 1:8
    [d,j]   = max(u(33:end - 65));
    j       = j + 32;
    % j     = fix(rand*T);              % random times
    k       = (j - 32):(j + 64);        % perstimulus time (around j)
    u((j - 8):(j + 8)) = -Inf;          % eliminate from next max(u(:,1))
    ue(:,i) = Xe(k,:)*CVA.V(:,1);
    um(:,i) = Xm(k,:)*CVA.W(:,1);
    us(i)   = j;

end
j    = any(ue);
us   = us(j);
ue   = spm_detrend(ue(:,j));
um   = spm_detrend(um(:,j));

% plot points of interest on conditional density
%--------------------------------------------------------------------------
subplot(3,1,2)
uy    = get(gca,'Ylim'); hold on
for i = 1:length(us),plot([1 1]*us(i),uy,':'), end, hold off

%  show time locked (internal and external) fluctuations and their mean
%--------------------------------------------------------------------------
subplot(3,2,5)
pst   = (-32:64)*8;
plot(pst,ue,'c:',pst,um,'b:'),               hold on
plot(pst,mean(ue,2),'c',pst,mean(um,2),'b'), hold on
plot([0 0],get(gca,'YLim'),'--'),            hold off, axis square
xlabel('Time (milliseconds)', 'FontSize',12)
ylabel('Electrochemical response','FontSize',12)
title('Simulated ERP','FontSize',16)

% canonical correlations
%--------------------------------------------------------------------------
subplot(3,2,6)
bar(CVA.r,1/2,'c')
xlabel('Mode', 'FontSize',12)
ylabel('Correlation','FontSize',12)
title('Canonical correlations','FontSize',16), axis square

% Jacobian's and generalised synchronisation
%==========================================================================
spm_figure('GetWin','Jacobians');clf

% get Markov blanket indices the Jacobian
%--------------------------------------------------------------------------
xi    = spm_zeros(x); xi.v(:,e) = 1; iXe = find(spm_vec(xi));
xi    = spm_zeros(x); xi.q(:,m) = 1; iXm = find(spm_vec(xi));

% show results - Jacobians (of increasing order: 1 to n)
%--------------------------------------------------------------------------
[d,j] = max(CVA.v(:,1));
j     = j + (-8:8);
J     = spm_soup(Q(:,:,j),X(:,:,j),V(:,:,j),P);
J     = mean(J,3);
j     = [iXe;iXm];
% q   = 8;
% U   = blkdiag(CVA.V(:,1:q),CVA.W(:,1:q));  % eigenmodes (not used)

n     = 4;
for i = 1:n

    % all states
    %----------------------------------------------------------------------
    JJ    = J^i;
    subplot(n,2,(i - 1)*2 + 1)
    spy(abs(JJ) > 1e-2,'k')
    title(sprintf('%i-order coupling',i),'FontSize',16)
    xlabel('All states','FontSize',12)
    ylabel('All states','FontSize',12)

    % Internal and external states
    %----------------------------------------------------------------------
    JJ    = JJ(j,j);  % JJ     = pinv(U)*JJ(j,j)*U;
    subplot(n,2,(i - 1)*2 + 2)
    spy(abs(JJ) > 1e-2,'k')
    title(sprintf('%i-order coupling',i),'FontSize',16)
    xlabel('External and internal','FontSize',12)
    ylabel('External and internal','FontSize',12)

end

return


classdef ParadigmSSRCSP < ParadigmBase
    % Selected-Subjects Regularized CSP (SSRCSP)
    %
    % This paradigm implements SSRCSP [1], which is a generalization of the Common Spatial Patterns
    % algorithms to calibration data comprising multiple subjects (or recordings). To train a model
    % for a particular "goal" (or target) subject using auxiliary data from other subjects, this
    % algorithm attempts to find a subset of other subjects such that, when data from those subjects
    % is combined with data from the goal subject, the performance on the goal subject's data is
    % optimal. The combination of data from multiple subjects and from the goal subject is done by
    % shrinking the goal-subject covariance matrix towards the average covariance matrix of the
    % other subjects (within each class), using a regularization parameter. The subset selection
    % algorithm employed is Sequential Floating Forward Selection (SFFS) [2].
    %
    % References:
    % [1] Lotte, F., & Guan, C.
    %     "Regularizing common spatial patterns to improve BCI designs: unified theory and new algorithms."
    %     Biomedical Engineering, IEEE Transactions on, 58(2), 355-362, 2011.
    %
    % [2] Pudil, P., Ferri, F. J., Novovicova, J., & Kittler, J.
    %     "Floating search methods for feature selection with nonmonotonic criterion functions."
    %     In Pattern Recognition, Proceedings of the 12th IAPR International. Conference on (Vol. 2, pp. 279-283). (1994)
    %
    % Name:
    %   Selected-Subjects Regularized Common Spatial Patterns
    %
    %                            Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                            2014-02-06

    methods

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

        function defaults = machine_learning_defaults(self)
            % set up the default parameters for machine learning
            defaults = {'lda',0.01,'regularization','shrinkage'};
        end

        function model = calibrate(self,varargin)
            % calibrate an SSRCSP model from a corpus of training sets
            args = arg_define(varargin, ...
                arg_norep({'collection','Collection'}), ...
                arg_norep({'goal_identifier','GoalIdentifier'}), ...
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 10000]),'Number of CSP patterns (times two).'),...
                arg({'beta','CovarianceShrinkage'},0.5,[0 1],'Covariance shrinkage. This is the degree to which data from the goal subject is shrunken towards that of other subjects.'),...
                arg_sub({'flt','SignalProcessing'}, self.preprocessing_defaults(), @flt_pipeline, 'Signal processing stages. These parameters control filter stages that run on the signal level; they can be enabled, disabled and configured for the given paradigm. The prediction operates on the outputs of this stage.'), ...
                arg_sub({'ml','MachineLearning'},{'Learner',self.machine_learning_defaults()},@ml_train,'Machine learning stage of the paradigm. Operates on the feature vectors that are produced by the feature-extraction stage.'),...
                arg({'arg_dialogsel','ConfigLayout'},self.dialog_layout_defaults(),[],'Parameters displayed in the config dialog. Cell array of parameter names to display (dot-notation allowed); blanks are translated into empty rows in the dialog. Referring to a structure argument lists all parameters of that struture, except if it is a switchable structure - in this case, a pulldown menu with switch options is displayed.','type','cellstr','shape','row'));

            % get the data of the reference subject
            [reference,remaining] = utl_collection_closest(args.collection,args.goal_identifier);
            % preprocess each recording in the reference collection and concatenate them across epochs into a single set
            for r=1:length(reference)
                refsets{r} = exp_eval_optimized(flt_pipeline('signal',reference{r}.streams{1}, args.flt)); end
            refdata = exp_eval(set_joinepos(refsets{:}));

            % pre-process data of all other subjects
            otherdata = {};
            for s=1:length(remaining)
                if length(remaining{s}.streams) > 1
                    disp_once('Note: ParadigmMKLCSP will use only the first data stream of a recording (no support for multi-modal data).'); end
                % preprocess
                otherdata{s} = exp_eval_optimized(flt_pipeline('signal',remaining{s}.streams{1}, args.flt)); %#ok<*NODEF>
                if otherdata{s}.nbchan < args.patterns
                    error('CSP requires at least as many channels as you request output patterns. Please reduce the number of pattern pairs.'); end
            end

            % get the best subjects
            model.best_subjects = hlp_diskcache('featuremodels',@self.find_best_subjects,otherdata,refdata,args.patterns,args.ml);

            % calculate composite CSP
            covar = self.class_covariances(refdata);
            other_covar = self.class_covariances(exp_eval(set_joinepos(otherdata{model.best_subjects})));
            for k=1:2
                covar{k} = (1-args.beta)*covar{k} + args.beta*other_covar{k}; end
            [V,D] = eig(covar{1},covar{1}+covar{2}); P = inv(V); %#ok<NASGU>
            model.featuremodel.filters = V(:,[1:args.patterns end-args.patterns+1:end]);
            model.featuremodel.patterns = P([1:args.patterns end-args.patterns+1:end],:);

            % train predictive model
            model.predictivemodel = ml_train('data',{self.feature_extract(refdata,model.featuremodel),set_gettarget(refdata)}, args.ml);
            % set the filter graph based on the reference data
            model.tracking.filter_graph = refsets{end};
            % also store channel locations for model visualization
            model.chanlocs = refdata.chanlocs;
        end

        function predictions = predict(self,bundle,model)
            % extract features
            features = self.feature_extract(bundle.streams{1},model.featuremodel);
            % apply classifier
            predictions = ml_predict(features,model.predictivemodel);
        end

        function best_subjects = find_best_subjects(self,otherdata,refdata,patterns,ml)
            % find set of best subjects to include
            selected = {[]};
            remaining = {1:length(otherdata)};
            accuracy = {-Inf};
            n = 0;
            while n < length(otherdata)
                % find best subject to add
                best_accuracy = -Inf;
                best_index = NaN;
                for k = remaining{1+n}
                    acc = self.evaluate_subset(otherdata([selected{1+n} k]),refdata,patterns,ml);
                    if acc > best_accuracy
                        best_accuracy = acc;
                        best_index = k;
                    end
                end
                selected{1+n+1} = [selected{1+n} best_index];
                remaining{1+n+1} = setdiff(remaining{1+n},best_index);
                accuracy{1+n+1} = best_accuracy;
                n = n+1;
                % remove subjects
                while n > 2
                    % find best subject to remove
                    best_accuracy = -Inf;
                    best_index = NaN;
                    for k=selected{1+n}
                        acc = self.evaluate_subset(otherdata(setdiff(selected{1+n},k)),refdata,patterns,ml);
                        if acc > best_accuracy
                            best_accuracy = acc;
                            best_index = k;
                        end
                    end
                    if best_accuracy > accuracy{1+n-1}
                        selected{1+n-1} = setdiff(selected{1+n},best_index);
                        remaining{1+n-1} = [remaining{1+n} best_index];
                        accuracy{1+n-1} = best_accuracy;
                        n = n-1;
                    else
                        break;
                    end
                end
            end
            best_n = argmax([accuracy{:}]);
            best_subjects = selected{best_n};
        end

        function accuracy = evaluate_subset(self,trainset,testset,patterns,ml)
            % note: we are here implicitly weighting by the amount of training data per subject
            if iscell(trainset)
                trainset = exp_eval(set_joinepos(trainset{:})); end
            % train CSP on training set
            covar = self.class_covariances(trainset);
            [V,D] = eig(covar{1},covar{1}+covar{2}); %#ok<NASGU>
            featuremodel.filters = V(:,[1:patterns end-patterns+1:end]);
            % train classifier
            classifier = ml_train('data',{self.feature_extract(trainset,featuremodel),set_gettarget(trainset)},ml);
            % test on test set
            accuracy = -ml_calcloss('auto',set_gettarget(testset),ml_predict(self.feature_extract(testset,featuremodel),classifier));
        end

        function covar = class_covariances(self,dataset)
            % calculate per-class covariance matrices
            for k=1:2
                classdata = exp_eval(set_picktrials(dataset,'rank',k));
                covar{k} = (classdata.data(:,:) * classdata.data(:,:)') / (size(classdata.data,2)*size(classdata.data,3));
                covar{k}(~isfinite(covar{k})) = 0;
            end
        end

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

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

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

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

    end
end

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


function f_x = ParFor10(in1)
%PARFOR10
%    F_X = PARFOR10(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    31-Mar-2020 17:03:12

r = in1(:,1);
s = in1(:,13);
u = in1(:,19);
ut = in1(:,20);
ux = in1(:,21);
uxx = in1(:,22);
uy = in1(:,23);
uyy = in1(:,24);
z = in1(:,7);
t2 = u.^2;
f_x = r.*-4.845016406907234+s.*6.96612441301113+ut.*5.457984690783633e-1-ux.*1.530748504418966e-2+uxx.*2.135644249305187-uy.*3.570878181824355e-2+uyy.*5.02823932467436-z.*1.323504040227272e2+r.*u.*6.191151455437648-s.*u.*9.105578580027213-t2.*u.*1.077295023277402e2+u.*ut.*5.65024831920482e-2+u.*ux.*1.830355084143775e-2-u.*uxx.*4.132096419489244+u.*uy.*4.442896435307375e-2-u.*uyy.*6.867807677976089+u.*z.*1.83605552916415e2+4.028421339788474e1;


function ehmm = hsupdate_ehmm(Xi,Gamma,T,ehmm)
%
% updates hidden state parameters of an HMM
%
% INPUT:
%
% Xi     probability of past and future state cond. on data
% Gamma  probability of current state cond. on data
% T      length of observation sequences
% hmm    single hmm data structure
%
% OUTPUT
% hmm    single hmm data structure with updated state model probs.
%
% Author: Diego Vidaurre, OHBA, University of Oxford

K = ehmm.K;

if isempty(Xi)  % non-exact estimation
    order = (sum(T)-size(Gamma,1)) / length(T);
    Xi = zeros(sum(T-1-order),K,2,2);
    for j = 1:length(T)
        indG = (1:(T(j)-order)) + sum(T(1:j-1)) - (j-1)*order;
        indXi =  (1:(T(j)-order-1)) + sum(T(1:j-1)) - (j-1)*(order+1);
        for k = 1:K
            g = [Gamma(indG,k) (1-Gamma(indG,k))];
            for t = 1:length(indXi)
                xi = g(t,:)' * g(t+1,:);
                xi = xi / sum(xi(:));
                Xi(indXi(t),k,:,:) = xi;
            end
        end
    end
end
if length(size(Xi))==4
    Xi = permute(sum(Xi),[2 3 4 1]);
end

% transitions
for k = 1:K
    ehmm.state(k).Dir2d_alpha = permute(Xi(k,:,:),[2 3 1]) ...
        + ehmm.state(k).prior.Dir2d_alpha;
    ehmm.state(k).P = zeros(2);
    for j = 1:2
        PsiSum = psi(sum(ehmm.state(k).Dir2d_alpha(j,:)));
        for j2 = 1:2
            ehmm.state(k).P(j,j2) = ...
                exp(psi(ehmm.state(k).Dir2d_alpha(j,j2))-PsiSum);
        end
        ehmm.state(k).P(j,:) = ehmm.state(k).P(j,:) ./ sum(ehmm.state(k).P(j,:));
    end
end

% initial state is always OFF for all chains
end


function [um] = au2um(au)
% Convert length from astronomical units to microns.
% Chad A. Greene 2012
um = au*1.49597870691e+17;


%DIAGNOSTIC TOOLS (in the diag-folder):
%
% Convergence diagnostics
%   PSRF     - Potential Scale Reduction Factor
%   CPSRF    - Cumulative Potential Scale Reduction Factor
%   MPSRF    - Multivariate Potential Scale Reduction Factor
%   CMPSRF   - Cumulative Multivariate Potential Scale Reduction Factor
%   IPSRF    - Interval-based Potential Scale Reduction Factor
%   CIPSRF   - Cumulative Interval-based Potential Scale Reduction Factor
%   KSSTAT   - Kolmogorov-Smirnov goodness-of-fit hypothesis test
%   HAIR     - Brooks' hairiness convergence diagnostic
%   CUSUM    - Yu-Mykland convergence diagnostic for MCMC
%   SCORE    - Calculate score-function convergence diagnostic
%   GBINIT   - Initial iterations for Gibbs iteration diagnostic
%   GBITER   - Estimate number of additional Gibbs iterations
%
% Time series analysis
%   ACORR      - Estimate autocorrelation function of time series using xcorr
%   ACORR2     - Estimate autocorrelation function of time series using fft
%   ACORRTIME  - Estimate autocorrelation evolution of time series (simple)
%   GEYER_ICSE - Compute autocorrelation time tau using Geyer's
%                initial convex sequence estimator
%                (requires Optimization toolbox)
%   GEYER_IMSE - Compute autocorrelation time tau using Geyer's
%                initial monotone sequence estimator
%
% Survival model criteria
%   AUCS       - Compute area under curve for survival model
%   AUCT       - Compute area under curve for survival model at given time
%   EXT_AUC    - Compute Extended AUC proposed by Chambless et al (2011)
%   HCS        - Compute Harrell's C for survival model at given time
%   HCT        - Compute Harrel's C for survival model at several time points
%   IDIS       - Integrated Discrimination Improvement between two models
%   RSQR       - R^2 statistic given probabilities at time point T
%
% Kernel density estimation etc.:
%   KERNEL1  - 1D Kernel density estimation of data
%   KERNELS  - Kernel density estimation of independent components of data
%   KERNELP  - 1D Kernel density estimation, with automatic kernel width
%   NDHIST   - Normalized histogram of N-dimensional data
%   HPDI     - Estimates the Bayesian HPD intervals
%
% Misc:
%   CUSTATS   - Calculate cumulative statistics of data
%   GRADCHEK  - Checks a user-defined gradient function using finite
%               differences.
%   DERIVATIVECHECK - Compare user-supplied derivatives to
%                     finite-differencing derivatives.
%


function f = chebfun3double(f, op, dom, pref, isEqui)
%CHEBFUN3DOUBLE   CHEBFUN3 constructor for discrete tensor of values
%   This algorithm is equivalent to chebfun3classic for double inputs.

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

% The input is a discrete tensor of values.
pseudoLevel = pref.cheb3Prefs.chebfun3eps;
tech = pref.tech();

f = chebfun3();
if ( ~isEqui && numel(op) == 1 )
    f = constructor(f, @(x,y,z) op + 0*x, dom);
    return;
end

% N.B. We cannot detect if MESHGRID was used to generate values unless we
% know also the (x,y,z) points used to generate those values. If we knew
% beforehand that ALL users WILL generate their tensor of values ONLY from
% meshgrid pts, all we need is to say vals = permute(vals,[2 1 3]); to
% generate a tensor corresponding to ''meshgrid'', in which case a copy of
% ``tensorGrid`` should also be used accordingly. op = permute(op,[2 1 3]);
if ( ~isEqui )
    m = size(op, 1);
    n = size(op, 2);
    p = size(op, 3);
    out = tech.tensorGrid([m, n, p], dom);
    xx = out{1};
    yy = out{2};
    zz = out{3};
else
    % Equispaced points from ndgrid, not meshgrid!
    x = linspace(dom(1), dom(2), size(op, 1));
    y = linspace(dom(3), dom(4), size(op, 2));
    z = linspace(dom(5), dom(6), size(op, 3));
    [xx, yy, zz] = ndgrid(x, y, z);
end

% Calculate a tolerance and find numerical rank to this tolerance: The
% tolerance assumes the samples are generated by NDGRID from a function.
% It depends on the size of the sample tensor, hscale of domain, vscale of
% the samples, condition number of the function, and the accuracy target in
% chebfun3 preferences.
[relTol, absTol] = getTol3D(xx, yy, zz, op, max(size(op)), dom, pseudoLevel);
pref.chebfuneps = relTol;

% Perform 3D ACA with complete pivoting:
factor = 0;
[colsValues, rowsValues, pivotVals2D, ~, tubesValues, pivotVals3D, ~, ~, ...
    ~] = completeACA3D(op, absTol, factor, dom, pref);

sepRank = numel(pivotVals3D); % first separation rank
diagValues2D = cell(sepRank, 1);
for k=1:sepRank
    diagValues2D{k} = diag(1./pivotVals2D{k});
end

% BTD ---> Tucker compression:
[core, colsValues, rowsValues] = btd2tucker(colsValues, rowsValues, ...
    diagValues2D, pivotVals3D, absTol);

% Construct a CHEBFUN3 object and call a sampleTest.
if ( ~isEqui )
    f.cols = chebfun(colsValues, dom(1:2), pref);
    f.rows = chebfun(rowsValues, dom(3:4), pref);
    f.tubes = chebfun(tubesValues, dom(5:6), pref);
else
    f.cols = chebfun(colsValues, dom(1:2), 'equi', pref);
    f.rows = chebfun(rowsValues, dom(3:4), 'equi', pref);
    f.tubes = chebfun(tubesValues, dom(5:6), 'equi', pref);
end
f.core = core;
f.domain = dom;
return

end

%%
function [relTol, absTol] = getTol3D(xx, yy, zz, vals, grid, dom, ...
    pseudoLevel)

relTol = 2*grid^(4/5) * pseudoLevel; % this should be vscale and hscale invariant
vscale = max(abs(vals(:)));
[m,n,p] = size(vals);
% Remove some edge values so that df_dx, df_dy and df_dz have the same size.
% xx changes in the first mode:
df_dx = diff(vals(:, 1:n-1, 1:p-1), 1, 1) ./ diff(xx(:, 1:n-1, 1:p-1), 1, 1);
% yy changes row-wise (2nd mode):
df_dy = diff(vals(1:m-1, :, 1:p-1), 1, 2) ./ diff(yy(1:m-1, :, 1:p-1), 1, 2);
% zz changes tube-wise (3rd mode):
df_dz = diff(vals(1:m-1, 1:n-1, :), 1, 3) ./ diff(zz(1:m-1, 1:n-1, :), 1, 3);
gradNorms = [max(abs(df_dx(:))), max(abs(df_dy(:))), max(abs(df_dz(:)))];
% A vector of gradient information over the domain.
if ( isempty(gradNorms) )
    % This happens if the input in not a trivariate function in which case
    % we basically disable using gradient information:
    gradNorms = 1;
end
domDiff = [diff(dom(1:2)) diff(dom(3:4)) diff(dom(5:6))];
absTol = max(max(gradNorms.*domDiff), vscale) * relTol;
% absTol should depend on the vscale of the function while it also uses
% derivative information to prevent issues like the one mentioned in
% https://github.com/chebfun/chebfun/issues/1491.

end

%%
function [colsBtd, rowsBtd, pivotValues2D, pivotIndices2D, fibers, ...
    pivotValues3D, pivotIndices3D, ifail3D, ifail2D] = completeACA3D(A, ...
    tol, factor, dom, pref)
%   Non-adaptive (fixed-size) MACA, i.e., a 3D analogue of Gaussian
%   elimination with complete pivoting.
%
%   INPUTS:     A:        A given tensor of function values at 3D chebpts.
%
%               tol:      A given tolerance on the magnitude of the pivots.
%
%               factor:   The ratio between the width of A and the number
%                         of iterations (rank) allowed in Gaussian elimination.
%
%  OUTPUTS:     colsBtd: A cell-array containing skeleton columns of slices
%                        in block term decomposition.
%
%               rowsBtd: A cell-array containing skeleton rows of slices in
%                        block term decomposition.
%
%               pivotValues2D: A cell array containing the values of pivots
%                              in 2D ACAs.
%
%               pivotIndices2D: A cell array containing indices i.e.,
%                               locations of of 2D pivot points.
%
%                       fibers: A matrix of size n1 x ITER. Each of its columns
%                             contains the values of the updated (residue)
%                             tensor at the pivot fiber.
%
%               pivotValues3D: A row vector containing the values of the
%                             pivot entries during the iterations.
%
%               pivotIndices3D: A matrix of size rank x 3, where rank = iter.
%                         Each of its rows contain the index of one 3D pivotValues.
%
%                      ifail: We fail if iter >= (width/factor).

% Developer Note: The output of this code should satisfy the following
% slice decomposition:
%       AA \approx temp,
% where AA is a copy of A from input, and temp is computed as follows:
%   temp = zeros(size(A2));
%   for i = 1:3,
%    temp = temp + chebfun3.outerProd(slices(:,:,i),fibers(:,i)./pivotValues3D(i));
%   end
%   norm(AA(:) - temp(:))
%
% An analogous BTD decomposotion should also hold.

pseudoLevel = pref.cheb3Prefs.chebfun3eps;
tech = pref.tech();

% Set up output variables.
[n1, n2, n3] = size(A);
width = min(n3, n1*n2);        % Use to tell us how many pivots we can take
                               % See Developer note in the following.
pivotValues3D = zeros(1);      % Store an unknown number of Pivot values
pivotIndices3D = zeros(1, 3);  % Store (col, row, tube) = entries of pivot location
ifail3D = 1;                   % Assume we fail in 3D ACA
ifail2D = 1;                   % Assume we also fail in the 2D ACAs
globalTol = [];
sliceDim = [1 2];              % See Developer note in the following.

% Main algorithm
iter = 0;                      % Count number of interpolated rows/slices.
[infNorm, ind] = max(abs(reshape(A, numel(A), 1))); % Complete pivoting
[col, row, tube] = ind2sub(size(A), ind);

scl = infNorm;
% If the function is the zero function.
if ( scl == 0 )
    pivotValues3D = 0;
    fibers = 0;
    colsBtd{1} = 0;
    rowsBtd{1} = 0;
    pivotValues2D = 0;
    pivotIndices2D = [0 0];
    ifail3D = 0;
    ifail2D = 0;
else
    fibers(:,1) = zeros(size(A, 3), 1);
    colsBtd{1} = zeros(size(A, sliceDim(1)), 1);
    rowsBtd{1} = zeros(size(A, sliceDim(2)), 1);
    pivotValues2D{1} = 0;
    pivotIndices2D{1} = [0 0];
    slices(:,:,1) = zeros(size(A,sliceDim(1)), size(A, sliceDim(2)), 1);
end
dom2D = dom(1:4);

while ( ( infNorm > tol ) && ( iter < width / factor) ...
        && ( iter < width ) )
    fibers(:, iter+1) = A(col, row, :);  % Extract skeleton tubes. Each
    % column in "fibers" is N3 x 1.

    slices(:,:,iter+1) = A(:,:,tube);    % Extract skeleton slices to be
    % decomposed further. Each slice in "slices" is N1 x N2.

    % Developer Note: As the above lines show, we are always separating the
    % last variable from the first two. The point is that the function
    % handle has already been permuted outside this subroutine and
    % therefore the tensor A here contains values of the permuted function.
    % In this sense, here we are in essense separating the variable chosen
    % by the dimension clustering step, and not necessarily the last
    % variable z.

    PivVal3D = A(col, row, tube);        % = f(X(col), Y(row), Z(tube)) in
                                         % the 1st iteration.

    % Use the first slice to compute globalTol for 2D ACAs applied to all
    % slices.
    if ( iter == 0 )
        out = tech.tensorGrid([n1, n2], dom2D);
        xx2D = out{1};
        yy2D = out{2};
        globalTol = GetTol2D(xx2D, yy2D, slices(:,:,1), dom2D, pseudoLevel);
    end

    % Apply 2D ACA to each slice to form columns and rows in block term
    % decomposition:
    [colsBtd{iter+1}, pivotValues2D{iter+1}, rowsBtd{iter+1}, ...
        pivotIndices2D{iter+1}, ifail2DIter] = ...
        chebfun2ACA(slices(:, :, iter+1), globalTol, factor);

    % Developer Note: Since we use globalTol for slices after 1st
    % iteration, it might be that these 2D ACA's don't fail, while with a
    % localTol they would fail. So, it is auaully the 1st slice which shows
    % whether or not we got the 2D rank right. So, just use that one:
    if ( iter == 0 )
        ifail2D = ifail2DIter;
    end

    % Update the tensor, i.e., compute the residual tensor:
    A = A - chebfun3.outerProd(colsBtd{iter+1} * ...
        (diag(1./(pivotValues2D{iter+1}))) * (rowsBtd{iter+1}.'), ...
        fibers(:,iter+1) ./ PivVal3D);
    % Equivalently, we have:
    % A = A - chebfun3.outerProd(slices(:,:,iter+1),fibers(:,iter+1)./PivVal3D);

    % Keep track of progress in 3D ACA:
    iter = iter + 1;              % One more fiber and slice are removed from A
    pivotValues3D(iter) = PivVal3D;           % Store pivot value in 3D ACA
    pivotIndices3D(iter, :)=[col row tube];    % Store pivot location in 3D ACA

    % Find next 3D pivot value and its location:
    [infNorm, ind] = max(abs(A(:)));
    [col, row, tube] = ind2sub(size(A), ind);
end

if ( ( iter > 0 ) && ( all(pivotValues2D{iter} == 0) ) )
    % If the last 2D pivot was zero, remove it
    colsBtd=colsBtd(1:iter-1);
    rowsBtd=rowsBtd(1:iter-1);
    pivotValues2D = pivotValues2D(1:iter-1);
    pivotIndices2D = pivotIndices2D(1:iter-1);
    fibers = fibers(:, 1:iter-1);
    pivotValues3D = pivotValues3D(1:iter-1);
    pivotIndices3D=pivotIndices3D(1:iter-1,:);
    infNorm = 0; % If the last 2D pivot was zero, infNorm will be NaN and
    % it makes the next statement result in ifail3D \neq 0; So, put it 0 to
    % get ifail3D = 0;
end

if ( infNorm <= tol )
    ifail3D = 0;                               % We didn't fail in 3D ACA.
    if ( iter == 0 )
        ifail2D = 0;
    end
end
if ( iter > width/factor )
    ifail3D = 1;                               % We did fail in 3D ACA.
end

end
%%

%%
function absTol = GetTol2D(xx, yy, vals, dom, pseudoLevel)
% GETTOL2D   Calculate a tolerance for the Chebfun2 constructor.
%
%  This is the 2D analogue of the tolerance employed in the chebtech
%  constructors. It is based on a finite difference approximation to the
%  gradient, the size of the approximation domain, the internal working
%  tolerance, and an arbitrary (4/5) exponent.

[m, n] = size(vals);
grid = max(m, n);
relTol = grid^(4/5) * pseudoLevel; % this should be vscale and hscale invariant

% Remove some edge values so that df_dx and df_dy have the same size.
% xx is generated by ndgrid, i.e., xx changes in the first mode:
dfdx = diff(vals(:, 1:n-1), 1, 1) ./ diff(xx(:, 1:n-1), 1, 1);
% yy is generated by ndgrid, i.e., yy changes row-wise (2nd mode):
dfdy = diff(vals(1:m-1, :), 1, 2) ./ diff(yy(1:m-1, :), 1, 2);
gradNorms = [max(abs(dfdx(:))), max(abs(dfdy(:)))];
% A vector of gradient information over the domain.
if ( isempty(gradNorms) )
    % This happens if the input in not a trivariate function.
    gradNorms = 1;
end

vscale = max(abs(vals(:)));
domDiff = [diff(dom(1:2)) diff(dom(3:4))];
absTol = max(max(gradNorms.*domDiff), vscale) * relTol;
end

%%
function [col, pivotVals, row, pivotLoc, ifail2D] = chebfun2ACA(op, ...
    tol, factor)
% Perform GE with complete pivoting:

if ( factor ~= 0 )
    % FACTOR in the 3D steps is either 0 (in case of constructionFromDoubles)
    % or 2sqrt(2) otherwise. For 2D steps however, we are happy with
    % FACTOR = 0 or 2 as in Chebfun2. This IF conditional, makes it
    % possible to rewrite FACTOR in 2D steps, but at the same time keeping
    % it zero fro constructionFromDoubles.
    factor = 2;
end
[pivotVals, pivotLoc, row, col, ifail2D] = completeACA2D(op, tol, factor);
end

%%
function [pivotValue, pivotElement, rows, cols, ifail2D] = ...
    completeACA2D(A, tol, factor)
% 2D ACA with complete pivoting which is the continuous analogue of
% Gaussian elimination with complete pivoting.
% We attempt to adaptively find the numerical rank of function in the 2D
% level. This is _almost_ the same as the one in chebfun2/constructor.

% Set up output variables.
[nx, ny] = size(A);
width = min(nx, ny);        % Use to tell us how many pivots we can take.
pivotValue = zeros(1);      % Store an unknown number of Pivot values.
pivotElement = zeros(1, 2); % Store (j,k) entries of pivot location.
ifail2D = 1;                  % Assume we fail.

% Main algorithm
zRows = 0;                  % count number of zero cols/rows.
[infNorm, ind] = max(abs(reshape(A, numel(A), 1)));
[row, col] = chebfun3.myind2sub(size(A) , ind);

% Bias toward diagonal for square matrices (see reasoning below):
if ( ( nx == ny ) && ( max(abs(diag(A))) - infNorm) > -tol )
    [infNorm, ind] = max(abs(diag(A)));
    row = ind;
    col = ind;
end

scl = infNorm;
% If the function is the zero function
if ( scl == 0 )
    pivotValue = 0;
    cols = 0;
    rows = 0;
    ifail2D = 0;
else
    cols(:,1) = zeros(size(A, 1), 1);
    rows(1,:) = zeros(1, size(A, 2));
end

while ( ( infNorm > tol ) && ( zRows < width / factor) ...
        && ( zRows < min(nx, ny) ) )

    cols(:, zRows+1) = A(:, col);             % Extract skeleton columns
    rows(zRows+1, :) = A(row, :);             % Extract skeleton rows
    PivVal = A(row, col);
    A = A - cols(:, zRows+1)*(rows(zRows+1,:)./PivVal); % One step of GE

    % Keep track of progress.
    zRows = zRows + 1;                       % One more row is interpolated
    pivotValue(zRows) = PivVal;              % Store value of 2D pivot
    pivotElement(zRows, :)=[row col];        % Store index of 2D pivot

    % Find value and index of next 2D pivot
    [infNorm, ind] = max(abs(A(:))); % Slightly faster
    [row, col] = chebfun3.myind2sub(size(A), ind);

    % Have a bias towards the diagonal of A, so that it can be used as a test
    % for nonnegative definite functions. (Complete GE and Cholesky are the
    % same as nonnegative definite functions have an absolute maximum on the
    % diagonal, except there is the possibility of a tie with an off-diagonal
    % absolute maximum. Bias toward diagonal maxima to prevent this.)
    if ( ( nx == ny ) && ( max(abs(diag(A))) - infNorm) > -tol )
        [infNorm, ind] = max(abs(diag(A)));
        row = ind;
        col = ind;
    end
end

if ( infNorm <= tol )
    ifail2D = 0;                               % We didn't fail in 2D ACA
end
if ( zRows >= (width/factor) )
    ifail2D = 1;                               % We did fail in 2D ACA
end

rows = rows.';                               % To unify all the columns,
                                             % rows and tubes, store
                                             % skeleton rows also as column
                                             % vectors.
end

%%
function [core, colsTucker, rowsTucker] = btd2tucker(colsValues, ...
    rowsValues, diagValues2D, pivotVals3D, absTol)
allCols = [];
allRows = [];
allDiags = [];
nn = numel(pivotVals3D);
sizeIndex = zeros(nn, 1);

for kkk = 1:nn
    sizeIndex(kkk) = size(rowsValues{kkk}, 2);
    allCols = [allCols, colsValues{kkk}];
    allRows = [allRows, rowsValues{kkk}];
    allDiags = [allDiags; diag(diagValues2D{kkk})];
end
sizeIndex = cumsum([0; sizeIndex]);

% Compress allCols and allRows:
if ( size(allCols, 2) > 1 )
    [Su, ~, Vu, colsTucker] = completeACA2D(allCols, absTol, 0);
    % factor = 0, because we want the ACA to be applied even if op is not
    % low-rank. In contrast to Chebfun2, we now have
    % allCols = colsTucker * diag(1./Su) * Vu'.
    % Moreover, we use the same absTol to chop columns, the same tolerance
    % used to chop fibers so that ranks are less inconsistent for symmetric
    % functions.
    Vu = Vu*diag(1./Su);
else
    colsTucker = allCols;
    Vu = 1;
end

if ( size(allRows, 2) > 1 )
    [Sv, ~, Vv, rowsTucker] = completeACA2D(allRows, absTol, 0);
    Vv = Vv*diag(1./Sv);
else
    rowsTucker = allRows;
    Vv = 1;
end

% Form the core tensor:
core = zeros(size(colsTucker, 2), size(rowsTucker, 2), nn);
for kkk = 1:nn
    core(:,:,kkk) = Vu(sizeIndex(kkk)+1:sizeIndex(kkk+1), :).' * ...
        diag(allDiags(sizeIndex(kkk)+1:sizeIndex(kkk+1))) * ...
        Vv(sizeIndex(kkk)+1:sizeIndex(kkk+1), :)./pivotVals3D(kkk);
end

if ( (max(abs(allCols(:))) == 0) || (max(abs(allRows(:))) == 0) )
    % zero input
    core = 0;
end

end


function [res,qualMeasOut]=MLEM(proj,geo,angles,niter,varargin)
%MLEM solves the tomographic problem by using Maximum Likelihood Expectation
% Maximisation algorithm.
%
%   MLEM(PROJ,GEO,ALPHA,NITER,opt) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry described in GEO, using NITER iterations.
%
% 'verbose': Get feedback or not. Default: 1
%
% 'init':    Describes different initialization techniques.
%             •  'none'     : Initializes the image to ones (default)
%             •  'FDK'      : Initializes image to FDK reconstruction
%
% 'QualMeas':    Asks the algorithm for a set of quality measurement
%                parameters. Input should contain a cell array of desired
%                quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                These will be computed in each iteration.
%
% 'groundTruth':  An image as ground truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
[verbose,res,QualMeasOpts,gpuids,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<2 && measurequality
    warning("Image metrics requested but none caught as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

res = max(res,0);

% Back-projection weight, V
V = Atb(ones(size(proj),'single'),geo,angles,'matched','gpuids',gpuids);
V(V<=0.) = inf;

for ii=1:niter
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = res; % only store if necessary
    end
    if (ii==1);tic;end

    den = Ax(res,geo,angles,'gpuids',gpuids);
    den(den<=0.)=inf;

    imgupdate = Atb(proj./den, geo,angles,'matched','gpuids',gpuids)./V;
    res = max(res.*imgupdate,0.);

    if measurequality
        qualMeasOut(:,ii)=Measure_Quality(res_prev,res,QualMeasOpts);
    end

    if (ii==1)&&(verbose==1)
        expected_time=(toc)*niter;
        disp('MLEM');
        disp(['Expected duration   :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

%% Parse inputs
function [verbose,f0,QualMeasOpts,gpuids,gt]=parse_inputs(proj,geo,angles,argin)
opts = {'verbose','init','qualmeas','gpuids','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:MLEM:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:MLEM:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option is not default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:MLEM:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        % % % % % % % Verbose
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE: Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
        % Initial image
        %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=ones(geo.nVoxel','single');
            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid init')
                end
            end
        % Image Quality Measure
        %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid quality measurement parameters');
                end
            end
        % GPUIDS
        % =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:MLEM:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in MLEM()']);
    end
end
end


function Dual_TVL1_optic_flow( I0,I1,     u1, u2, nx, ny,     tau,    lambda, theta,   warps,  epsilon,verbose,iflagMedian)
size = round(nx * ny);
l_t = lambda * theta;
if isa(u1,'cell') && isa(u2,'cell')
   u1 = cell2mat(u1);
   u2 = cell2mat(u2);
end
MAX_ITERATIONS= 300;
PRESMOOTHING_SIGMA =0.8;
GRAD_IS_ZERO = 1E-10;
[I1x, I1y] = centered_gradient(I1, 0, 0, nx, ny);
  for  i = 1: size
        p11(i) =0.0;
        p12(i) = 0.0;
        p21(i) =0.0;
        p22(i) = 0.0;
  end
  rho_c = zeros(1,size);
  grad = zeros(1,size);
  for  warpings = 1: warps
      I1w = bicubic_interpolation_warp(I1,  u1, u2, 0,  nx, ny, 1);
      I1wx =  bicubic_interpolation_warp(I1x, u1, u2, 0, nx, ny, 1);
     I1wy = bicubic_interpolation_warp(I1y, u1, u2, 0, nx, ny, 1);
      for  i = 1:size
           Ix2 = I1wx(i) * I1wx(i);
             Iy2 = I1wy(i) * I1wy(i);

            grad(i) = (Ix2 + Iy2);
             sizes = [length(u2),length(I1w),length(I1wx),length(u1),length(I1wy),length(I0)];
if i <= min(sizes)
            rho_c(i) = (I1w(i) - I1wx(i) * u1(i)- I1wy(i) * u2(i) - I0(i));
end
      end
     n = 0;
       error = Inf;
        while (error > epsilon * epsilon && n < MAX_ITERATIONS)
            n = n +1;

            for  i = 1 : size
                 sizes = [length(u2),length(I1w),length(I1wx),length(u1),length(I1wy),length(I0)];
if i <= min(sizes)
               rho = rho_c(i)+ (I1wx(i) * u1(i) + I1wy(i) * u2(i));

               d1 = 0.0;  d2 = 0.0;

                if (rho < - l_t * grad(i))
                    d1 = l_t * I1wx(i);
                    d2 = l_t * I1wy(i);
                 else
                    if (rho > l_t * grad(i))
                        d1 = -l_t * I1wx(i);
                        d2 = -l_t * I1wy(i);
                     else
                        if (grad(i) < GRAD_IS_ZERO)
                            d1 = 0;
                            d2 = 0;
                        else
                             fi = -rho/grad(i);
                            d1 = fi * I1wx(i);
                            d2 = fi * I1wy(i);
                        end

                    end
                end
end
if(i <= length(u1))
                v1(i) = u1(i) + d1;
end
if(i <= length(u2))
                v2(i) = u2(i) + d2;
end
            end
           div_p1 =  divergence(p11, p12, 0, nx ,ny);
           div_p2 = divergence(p21, p22, 0, nx ,ny);
            error = 0.0;

            for  i = 1:  size-1
                if(i <= length(u1))
                u1k = u1(i);
                u2k = u2(i);

                u1(i) = v1(i) + theta * div_p1(i);
                u2(i) = v2(i) + theta * div_p2(i);

                error = error + (u1(i) - u1k) * (u1(i) - u1k) +(u2(i) - u2k) * (u2(i) - u2k);
                end
            end
            error =  error / size;
           [u1x, u1y ] = forward_gradient(u1, 0, 0, nx ,ny);
           [u2x, u2y] =  forward_gradient(u2, 0, 0, nx ,ny);
          for (i = 1:  size-1)
                 taut = tau / theta;
                 if i < length(u1x)
                g1   = hypot(u1x(i), u1y(i));
                g2   = hypot(u2x(i), u2y(i));
                 end
               ng1  = 1.0 + taut * g1;
                 ng2  = 1.0 + taut * g2;

                p11(i) = (p11(i) + taut * u1x(i)) / ng1;
                p12(i) = (p12(i) + taut * u1y(i)) / ng1;
                p21(i) = (p21(i) + taut * u2x(i)) / ng2;
                p22(i) = (p22(i) + taut * u2y(i)) / ng2;
          end

        end

        %applay the filter and continue
end


function [Y, optinf] = bpdngrp(D, S, lambda, mu, g, opt)

% bpdngrp -- Basis Pursuit DeNoising with l2,1 group sparsity
%
%         argmin_x (1/2)||D*x - s||_2^2 + lambda*||x||_1 +
%                  mu * \sum_l ||G_l(x)||_2
%
%         The solution is computed using the ADMM approach (see
%         boyd-2010-distributed for details).
%
% Usage:
%       [Y, optinf] = bpdngrp(D, S, lambda, mu, g, opt)
%
% Input:
%       D           Dictionary matrix
%       S           Signal vector (or matrix)
%       lambda      Regularization parameter
%       mu          l2,1 regularization parameter
%       g           Vector containing index values indicating the
%                   group number for each dictionary element. The
%                   first group index is 1 (0 indicates no group).
%                   Numbers must be contiguous. Overlapping groups
%                   are not supported.
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       Y           Dictionary coefficient vector (or matrix)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose           Flag determining whether iteration status is displayed.
%                     Fields are iteration number, functional value,
%                     data fidelity term, l1 regularisation term, l2,1
%                     regularisation term, and primal and dual residuals
%                     (see Sec. 3.3 of boyd-2010-distributed). The value of
%                     rho is also displayed if options request that it is
%                     automatically adjusted.
%   MaxMainIter       Maximum main iterations
%   AbsStopTol        Absolute convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   RelStopTol        Relative convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   Y0                Initial value for Y
%   U0                Initial value for U
%   rho               ADMM penalty parameter
%   AutoRho           Flag determining whether rho is automatically updated
%                     (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod     Iteration period on which rho is updated
%   RhoRsdlRatio      Primal/dual residual ratio in rho update test
%   RhoScaling        Multiplier applied to rho when updated
%   AutoRhoScaling    Flag determining whether RhoScaling value is
%                     adaptively determined (see wohlberg-2015-adaptive). If
%                     enabled, RhoScaling specifies a maximum allowed
%                     multiplier instead of a fixed multiplier.
%   RhoRsdlTarget     Residual ratio targeted by auto rho update policy.
%   StdResiduals      Flag determining whether standard residual definitions
%                     (see Sec 3.3 of boyd-2010-distributed) are used instead
%                     of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam        Relaxation parameter (see Sec. 3.4.3 of
%                     boyd-2010-distributed)
%   AuxVarObj         Flag determining whether objective function is computed
%                     using the auxiliary (split) variable
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-07-10
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 6,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        l2,1      r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 64;
if opt.AutoRho,
  hstr = [hstr '   rho   '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Start timer
tstart = tic;

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
[Nr, Nc] = size(D);
Nm = size(S,2);
Nx = Nc*Nm;
Ng = max(g);
DTS = D'*S;
[luL, luU] = factorise(D, rho);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(Nc,Nm);
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(Nc,Nm);
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),

  % Solve X subproblem
  X = linsolve(D, rho, luL, luU, DTS + rho*(Y - U));

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink_groups(Xr + U, g, Ng, lambda/rho, mu/rho);

  % Update dual variable
  U = U + Xr - Y;

  % Objective function and convergence measures
  if opt.AuxVarObj,
    Jdf = sum(vec(abs(D*Y - S).^2))/2;
    Jl1 = sum(abs(vec(Y)));
    Jl21 = norm21(Y, g, Ng);
  else
    Jdf = sum(vec(abs(D*X - S).^2))/2;
    Jl1 = sum(abs(vec(X)));
    Jl21 = norm21(X, g, Ng);
  end
  Jfn = Jdf + lambda*Jl1 + mu*Jl21;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat;[k Jfn Jdf Jl1 Jl21 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, Jl21, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, Jl21, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if rsf ~= 1,
        [luL, luU] = factorise(D, rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

% End status display for verbose operation
if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink1(v, a)

  u = sign(v).*max(0, abs(v) - a);

return

function U = shrink2_col_vec(V, a)

  % Additional complexity here allows simultaenous shrinkage of a
  % set of column vectors
  n2v = sqrt(sum(V.^2,1));
  n2v(n2v == 0) = 1;
  U = bsxfun(@times, V, max(0, n2v - a)./n2v);

return

function U = shrink21(V, a, b)

  % See wohlberg-2012-local and chartrand-2013-nonconvex
  U = shrink2_col_vec(shrink1(V,  a), b);

return

function U = shrink_groups(V, g, Ng, a, b)

  U = zeros(size(V));
  U(g==0,:) = shrink1(V(g==0,:), a);
  for l = 1:Ng,
    U(g==l,:) = shrink21(V(g==l,:), a, b);
  end

return

function x = norm21(u, g, Ng)

  x = 0;
  for l = 1:Ng,
    x = x + sqrt(sum(u(g==l,:).^2, 1));
  end
  x = sum(x); % In case u is a matrix (i.e. not a column vector)

return

function [L,U] = factorise(A, c)

  [N,M] = size(A);
  % If N < M it is cheaper to factorise A*A' + cI and then use the
  % matrix inversion lemma to compute the inverse of A'*A + cI
  if N >= M,
    [L,U] = lu(A'*A + c*eye(M,M));
  else
    [L,U] = lu(A*A' + c*eye(N,N));
  end

return

function x = linsolve(A, c, L, U, b)

  [N,M] = size(A);
  if N >= M,
    x = U \ (L \ b);
  else
    x = (b - A'*(U \ (L \ (A*b))))/c;
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = 1;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 1;
  end

return


function [refMesh, distListIters, refVerticesIters] = averageMesh(meshList, varargin)
%AVERAGEMESH Compute average mesh from a list of meshes.
%
%   AVG = averageMesh(MESHLIST)
%
%   Example
%   averageMesh
%
%   See also
%     meshes3d

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2020-01-31, using Matlab 9.7.0.1247435 (R2019b) Update 2
% Copyright 2020-2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

%% Parse input values

% default values
nIters = 10;
verbose = false;

% parse input arguments
while length(varargin) > 1
    name = varargin{1};
    if ~ischar(name)
        error('require parameter name-value pairs');
    end

    if strcmpi(name, 'verbose')
        verbose = varargin{2};
    elseif strcmpi(name, 'nIters')
        nIters = varargin{2};
    else
        error(['Unknown parameter name: ' name]);
    end
    varargin(1:2) = [];
end

%% Initialisations

nMeshes = length(meshList);

% initialize kd-trees to accelerate nearest-neighbor searches
treeList = cell(nMeshes, 1);
for iMesh = 1:nMeshes
    treeList{iMesh} = KDTreeSearcher(meshList{iMesh}.vertices);
end

% choose arbitrary initial mesh
refMesh = struct('vertices', meshList{1}.vertices, 'faces', meshList{1}.faces);

refVerticesIters = cell(1, nIters);
distListIters = cell(1, nIters);

%% Main iteration

for iIter = 1:nIters
    if verbose
        fprintf('iter %d/%d\n', iIter, nIters);
    end
    refMesh = smoothMesh(refMesh);

    % create new array for average vertices
    newVerts = zeros(size(refMesh.vertices));
    distList = zeros(size(refMesh.vertices, 1), 1);

    % iterate over all meshes
    for iMesh = 1:nMeshes
        if verbose
            fprintf('    mesh %d/%d\n', iMesh, nMeshes);
        end

%         mesh = meshList{iMesh};
        inds = knnsearch(treeList{iMesh}, refMesh.vertices);

%         closest = mesh.vertices(inds,:);
        closest = treeList{iMesh}.X(inds,:);
        newVerts = newVerts + closest;
        distList = distList + sum((closest - refMesh.vertices).^2, 2);
    end

    % update new vertices
    newVerts = newVerts / nMeshes;
    refVerticesIters{iIter} = newVerts;
    refMesh.vertices = newVerts;

    % keep list of distances
    distList = sqrt(distList / nMeshes);
    distListIters{iIter} = distList;
end

% figure; drawMesh(refMesh, 'lineStyle', 'none', 'faceColor', [.5 .5 .5])
% axis equal; view(3); hold on; axis([-2.5 2.5 -2 2 -3.5 3.5]); light;
% lighting gouraud
% title('Average mesh');
% print(gcf, 'averageMesh_initial.png', '-dpng');


function [X_scal] = test_pretreatment(X,param)

% pretreatment for test data
%
% INPUT
% X:        data matrix (samples x variables)
% param:    output data structure from data_pretreatment routine
%
% OUTPUT
% X_scal:       pretreated data matrix (samples x variables)
%
% version 1.0 - september 2009
% Davide Ballabio
% Milano Chemometrics and QSAR Research Group
% www.disat.unimib.it/chm

a = param.a;
s = param.s;
m = param.m;
M = param.M;
pret_type = param.pret_type;

if strcmp(pret_type,'cent')
    amat = repmat(a,size(X,1),1);
    X_scal = X - amat;
elseif strcmp(pret_type,'scal')
    f = find(s>0);
    smat = repmat(s,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal = X(:,f)./smat(:,f);
elseif strcmp(pret_type,'auto')
    f = find(s>0);
    amat = repmat(a,size(X,1),1);
    smat = repmat(s,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal(:,f) = (X(:,f) - amat(:,f))./smat(:,f);
elseif strcmp(pret_type,'rang')
    f = find(M - m > 0);
    mmat = repmat(m,size(X,1),1);
    Mmat = repmat(M,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal(:,f) = (X(:,f) - mmat(:,f))./(Mmat(:,f) - mmat(:,f));
else
    X_scal = X;
end


function [jump_list, lid_ko, data,  running_mean] = getJumps(data, rate_scale)
% Return outliers in a relatively constant time series
%
% INPUT
%   data          data (column, eventually it can have a second column containing std)
%   rate_scale    24/rate_scale => used for scaling window size (1 => daily, 24 => hourly ...)
%
% OUTPUT
%
% SYNTAX
%   [jump_list, id_ko, data, running_mean] = getJumps(data, rate_scale)
%

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:        Andrea Gatti
%  Contributors:      Andrea Gatti
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------
    flag_smooth = true; % Apply a spline smoothing to rate (duration 3 days)
    spline_base = 7; % in days

    if size(data,2) == 2
        data_std = data(:,2);
        data = data(:,1);
    else
        data_std = 0;
    end
    med_data = median(data, 'omitnan');
    data = data - med_data;

    % Compute thresholds - empirical but it works (outliers should be less than 75% percent)
    thr_perc = 0.75;
    tmp = diff(data);
    tmp(abs(tmp) > 3 * perc(noNaN(abs(tmp)), thr_perc)) = nan;
    thr = std(tmp, 'omitnan');
    clear tmp;

    % Outlier detection
    if any(data_std)
        [lid_ko, data] = getOutliers([data data_std], thr);
    else
        [lid_ko, data] = getOutliers(data, thr);
    end

    flag_running = true;
    % Initialization

    % Jump parameters
    run_win = 5 * rate_scale;   % window size for running mean
    cumsum_thr = 2;             % above this threshold, cumulate residuals
    cum_thr = 6;                % above this threshold the cumulate TRIGGERS A JUMP

    % Recompute a running_threshold
    % when the noise is higher the discrimination of jumps is lower
    thr = movstd(diff(data), run_win, 'omitnan');
    if not(any(thr))
        thr(:) = nan2zero(std(data, 'omitnan'));
    end

    q_win = floor(run_win / 2);
    thr((q_win + 1) : end - q_win) = min(thr((2*q_win +1):end), thr(1:end - (2*q_win)));

    mthr = median(thr, 'omitnan');

    thr(isnan(zero2nan(thr))) = mthr;
    run_win_variable = round(max(1,sqrt((thr - mthr)/mthr)) * run_win);
    run_win_variable = [run_win_variable; run_win_variable(end)];
    %thr = mthr + 0.1 * (thr - mthr);

    mu = data(1); % initial mean
    i = 1 + find(not(isnan(data(2:end))), 1, 'first');  % running index on observations
    [s_plus, s_minus] = deal(zeros(size(data,1),1)); % sensors
    [n_plus, n_minus] = deal(uint16(zeros(size(data,1),1))); % epochs since sensor cumulation started

    running_mean = data; % running mean

    j = 1;           % running index on jumps
    jump_list = 0;   % list of jumps
    while i <= length(data)
        % if jumps are not detected
        if isnan(data(i))
            s_plus(i) = s_plus(i-1);        % keep the old sensor
            s_minus(i) = s_minus(i-1);      % keep the old sensor
            n_plus(i)  = iif(s_plus(i)  > 0, n_plus(i-1)  + 1, 0);
            n_minus(i) = iif(s_minus(i) < 0, n_minus(i-1) + 1, 0);
            running_mean(i) = mu;
            i = i + 1;
        else
            % while no jumps are detected and i is running on observations
            while i <= length(data) && s_plus(i-1) <= (cum_thr * thr(i-1)) && s_minus(i-1) >= -(cum_thr * thr(i-1))
                val = iif(abs(data(i) - mu) > 0.9*(cum_thr * thr(i-1)), mu, data(i));
                s_plus(i)  = max(0, s_plus(i-1)  + (data(i) - mu) - (cumsum_thr * thr(i-1)));
                s_minus(i) = min(0, s_minus(i-1) + (data(i) - mu) + (cumsum_thr * thr(i-1)));

                n_plus(i)  = iif(s_plus(i)  > 0 , n_plus(i-1)  + 1, 0);
                n_minus(i) = iif(s_minus(i) < 0 , n_minus(i-1) + 1, 0);

                if flag_running
                    mu = nan2zero(mean(data(max(jump_list(j)+1, i + 1 - run_win_variable(i)):i), 'omitnan'));
                end
                running_mean(i) = mu;
                i = i + 1;
            end
            if i <= length(data)
                n_d = i - 1;
                j = j + 1;
                jump_list(j) = iif(s_plus(n_d) > (cum_thr * thr(i-1)), n_d - n_plus(n_d), n_d - n_minus(i-1));
                mu = nan2zero(mean(data(max(jump_list(j)+1, n_d - run_win_variable(n_d)):(n_d)), 'omitnan'));

                jmp_magnitude = running_mean(jump_list(j)) - mu;
                Core.getLogger.addMessage(sprintf('Jump detected at epoch %d, it happened in epoch %d, length: %d, old mean %f.2, new mean %f.2, for a jump of %.2f\n', n_d, jump_list(j), n_d - jump_list(j) + 1, running_mean(jump_list(j)), mu, jmp_magnitude),100);
                if (i <= length(data))
                    [s_plus(i-1), s_minus(i-1)] = deal(0);
                    [n_plus(i-1), n_minus(i-1)] = deal(0);
                end
            end
        end
        mu = nan2zero(mean(data(max(jump_list(j)+1, i - run_win_variable(i-1)):(i-1)), 'omitnan'));
        running_mean(i-1) = mu;
    end

    % compute running mean backwords
    if flag_running
        running_smooth = running_mean;
        robustness_perc = 0.90;
        n_sigma = 6;

        % flag small jumps (1-2 epochs are too small to be considered "real jumps"
        for j = fliplr(find(diff(jump_list) < 3))
            id_ko = jump_list(j)+1:jump_list(j+1);
            data(id_ko) = nan;
            lid_ko(id_ko) = true;
            jump_list(j) = jump_list(j+1);
            jump_list(j+1) = [];
        end

        last_win_id = jump_list(j)+1 : size(data,1); % last window
        % Compute a trend "robust" using the robustness_perc of data
        %step_median(last_win_id) = median(data(last_win_id), 'omitnan');
        [tmp, trend] = strongDeTrend(data(last_win_id), robustness_perc, 1-((1-robustness_perc)/2), n_sigma);
        if flag_smooth && sum(not(isnan((data(last_win_id))))) > 5
            if any(trend); running_mean(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
            try
                [~, ~, ~, running_smooth(last_win_id)] = splinerMat(1:numel(last_win_id), data(last_win_id), ceil(spline_base * rate_scale), 1e-5, 1:numel(last_win_id)); % medium splines
            catch
                if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
            end
        else
            if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
        end
        for j = 2 : numel(jump_list)
            last_win_id = jump_list(j-1)+1 : (jump_list(j));
            %step_median(last_win_id) = median(data(last_win_id), 'omitnan');
            [tmp, trend] = strongDeTrend(data(last_win_id), robustness_perc, 1-((1-robustness_perc)/2), n_sigma);
            if flag_smooth && sum(not(isnan((data(last_win_id))))) > 5
                if any(trend); running_mean(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                try
                    [~, ~, ~, running_smooth(last_win_id)] = splinerMat(1:numel(last_win_id), data(last_win_id), ceil(spline_base * rate_scale), 1e-5, 1:numel(last_win_id)); % medium splines
                catch
                    if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                end
            else
                if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
            end
        end

        %  remove very small jumps - recursive
        flag_rem_jmp = true;
        while flag_rem_jmp
            % Check the magnitude of the jump, with both moving mean and splines, if it is too small is a false alarm
            jmp_magnitude = min(diff(running_mean(jump_list(2:end) + repmat([0 1]', 1, numel(jump_list)-1))), ...
                                diff(running_smooth(jump_list(2:end) + repmat([0 1]', 1, numel(jump_list)-1))));
            % Jump smaller than half the noise in that window of data will be ignored
            id_small_jmp = abs(jmp_magnitude)' < max(0.4,(thr(jump_list(2:end))));
            if any(id_small_jmp)
                jump_list(find(id_small_jmp) + 1) = [];  % remove jmp

                % recompute running mean
                last_win_id = jump_list(end)+1 : size(data,1);
                %step_median(last_win_id) = median(data(last_win_id), 'omitnan');
                % remove the trend before computing the running mean (this help in case of very steep intervals of data)
                [tmp, trend] = strongDeTrend(data(last_win_id), robustness_perc, 1-((1-robustness_perc)/2), n_sigma);
                if flag_smooth && sum(not(isnan((data(last_win_id))))) > 5
                    if any(trend); running_mean(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                    try
                        [~, ~, ~, running_smooth(last_win_id)] = splinerMat(1:numel(last_win_id), data(last_win_id), ceil(spline_base * rate_scale), 1e-5, 1:numel(last_win_id)); % medium splines
                    catch
                        running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend;
                    end
                else
                    if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                end
                for j = 2 : numel(jump_list)
                    last_win_id = jump_list(j-1)+1 : (jump_list(j));
                    [tmp, trend] = strongDeTrend(data(last_win_id), robustness_perc, 1-((1-robustness_perc)/2), n_sigma);
                    if flag_smooth && sum(not(isnan((data(last_win_id))))) > 5
                        if any(trend); running_mean(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                        try
                            [~, ~, ~, running_smooth(last_win_id)] = splinerMat(1:numel(last_win_id), data(last_win_id), ceil(spline_base * rate_scale), 1e-5, 1:numel(last_win_id)); % medium splines
                        catch
                            if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                        end
                    else
                        if any(trend); running_smooth(last_win_id) = movmean(tmp, run_win, 'omitnan') + trend; end
                    end
                end
            else
                flag_rem_jmp = false;
            end
        end
        running_mean = running_smooth;
    else
        last_win_id = jump_list(j)+1 : size(data,1);
        running_mean(last_win_id) = mean(data(last_win_id), 'omitnan');
    end

    running_mean = running_mean + med_data;
    data = data + med_data;
    % figure; plot(-s_minus); hold on; plot(s_plus); hold on; plot(cum_thr * thr)
end