%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of N-th order nonuniformly sampled bandlimited signals
% using digital filter banks
% Authors: S. K. Sindhi, K. M. M. Prabhu
%%%%%%%%%%%%%%%%%%%%%%%  with Knab window   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LF=13;w_c=0.85; BETA=4 Proposed/Prendergast and BETA=3 ITAMI gives best %
%%%%%%%%%%%%%%%%%%%%%%%  with kaiser_mine1  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LF=13;w_c=0.85; BETA=3 Proposed/Prendergast and BETA=3 ITAMI gives best %

% clear all;
% close all;
% clc;

display('Nth (2) order Reconstruction');

N = 2;                  % Nth order nonuniform sampling
TQ = 1;Fs = 1/TQ;                 % Nyquist Period

T = [1.5*TQ 3*TQ];      % Decimation Periods
K = 0.5*lcm(2*T(1), 2*T(2))/TQ; % number of samples in recurrent period
capT = K*TQ; % the full sampling period - of all samplers
M = capT./T;
ML = 400; % number of slices
w_c = 0.85;
NS = 100;  % Number of Sinusoids

LF = lcm(M(1),M(2))*2*K+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
n = -(LF-1)/2:1:(LF-1)/2;
Hd = firpm(LF-1,[0 w_c],[0 w_c*pi],'differentiator');
delayV = (LF-1)/2;

k = -(K-1):1:(K-1);
m = (0:1:(2*K-1))';
F = exp(1i*(pi/K).*kron(m,k));
std = [1e-6 1e-5 1e-4 1e-3 1e-2 1e-1];%5*1e-1];

serP = zeros(size(std));
serE = zeros(size(std));
serI = zeros(size(std));
serV = zeros(size(std));
serPr = zeros(size(std));
serJ = zeros(size(std));
serNO = zeros(size(std));

MCruns = 25;
MCruns1 = 25;

for tt = 1:length(std)
aa = 0;
display(tt);
for rrr = 1:MCruns1
taus = [0 1+std(tt)*randn]*TQ;
if or(taus(2)==1.5*TQ,taus(1)==taus(2))
    aa = aa+1;
    continue;
end
tausI = sort([taus(1) taus(2) T(1)+taus(1)]);
tauI = zeros(K,ML);
for p = 1:K
    tauI(p,:) = tausI(p)+(0:ML-1)*capT;
end;
a = zeros(1,K);
for p = 1:K
    a(p) = 1;
    for q = 1:K
        if q ~= p
                a(p) = a(p)/sin(pi*(tausI(p)-tausI(q))/capT);
        end;
    end;
end;
% c = sin(pi*(tausI(0+1))/capT);
% s = cos(pi*(tausI(0+1))/capT);
% b(1,1) = 0.5*(c+1i*s);
% c = sin(pi*(tausI(1+1))/capT);
% s = cos(pi*(tausI(1+1))/capT);
% b(1,2) = 0.5*(c+1i*s);
% b(3,:) = conj(b(1,:));
% b(2,:) = 0;

b = zeros(2*K-1,K);
c = -0.5*cos(pi*(tausI(1+1)+tausI(2+1))/capT);
s = -0.5*sin(pi*(tausI(1+1)+tausI(2+1))/capT);
b(1,1) = 0.5*(c+1i*s);
c = -0.5*cos(pi*(tausI(0+1)+tausI(2+1))/capT);
s = -0.5*sin(pi*(tausI(0+1)+tausI(2+1))/capT);
b(1,2) = 0.5*(c+1i*s);
c = -0.5*cos(pi*(tausI(0+1)+tausI(1+1))/capT);
s = -0.5*sin(pi*(tausI(0+1)+tausI(1+1))/capT);
b(1,3) = 0.5*(c+1i*s);
b(5,:) = conj(b(1,:));
b(2,:) = 0;
b(4,:) = conj(b(2,:));
c = 0.5*cos(pi*(tausI(2+1)-tausI(1+1))/capT);
b(3,1) = c;
c = 0.5*cos(pi*(tausI(2+1)-tausI(0+1))/capT);
b(3,2) = c;
c = 0.5*cos(pi*(tausI(1+1)-tausI(0+1))/capT);
b(3,3) = c;

r = tausI-TQ*(0:K-1);
rr = r(mod((0:ML*K-1),K)+1);

tauPr = zeros(K,ML);
for p = 1:K
    tauPr(p,:) = -tausI(p)+(0:ML-1)*capT;
end;
H = zeros(K,LF);
for i=1:K
%     H(i,:) = sinc(n-tausI(i)).*conv(sinc(n-tausI(i)),kaiser(LF,10).','same');
%     H(i,:) = sinc(n-tausI(i)).*knab(LF,3,-tausI(i)).';
    H(i,:) = sinc(n-tausI(i)).*kaiser_mine1(LF,3,-tausI(i));
end;
H = H(:,1:end-1);
EP = reshape(H.',K,length(H(1,:))/K,K);
capE = [];
for k=1:K
    temp = [];
    for i=1:K
        temp=[temp,toeplitz([EP(i,1,k),zeros(1,(length(H(1,:))/K)-1)],[EP(i,:,k),zeros(1,(length(H(1,:))/K)-1)])];
    end
    capE = [capE;temp];
end
d = ceil(size(capE,2)/(2*K));
P = kron(eye(K),[zeros(1,d),1,zeros(1,(size(capE,2)/K)-d-1)]);
R = P/capE;
% size(capE)
% size(zeros(LF-1,2*LF-2-K))
% size(R)
% size(zeros(K,LF-1))
% size(P)
% size(zeros(K,2*LF-2-K))
R = upsample(R.',K).';
rt = size(R,2)/K;
FR = zeros(K,rt);
for j=1:K
    for i=1:K
        temp = filter([zeros(1,K-i),1],1,R(i,(j-1)*rt+1:j*rt));
        FR(j,:) = FR(j,:)+temp;
    end
end

r = r.';
w_o = w_c*pi*TQ;
hJ = zeros(K,LF);
C = zeros(1,LF);
Nt = (LF-1)/2;
for i = 1:K
    C = -2*sin(w_o*(n-r(1+(mod(i-1-n,K)))'))./(pi*(n-r(1+(mod(i-1-n,K)))'));
    C(isnan(C)==1)=-2*w_o/pi;
    C = C.';
    S = zeros(LF,LF);
    for k = 1:LF
        S(k,:) = sin(w_o*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')))./(pi*(-Nt+k-1-r(1+(mod(i-1-(-Nt+k-1),K)))-(n-r(1+(mod(i-1-n,K)))')));
    end;
    S(isnan(S)==1)=w_o/pi;
    hJ(i,:) = -0.5*S\C;
end;

for pp = 1:MCruns;
Frq = rand(1,NS)*w_c/2;
Amp = rand(1,NS)/(sqrt(NS)*2);
Phi = rand(1,NS)*2*pi;
input = zeros(1,ML*K);
for k = 1:NS
  input = input + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K-1)*TQ+Phi(k));
end;
xp = zeros(N,ML*K);
for p = 1:N
    tau = taus(p)+(0:ML*M(p)-1)*T(p);
    x1 = zeros(1,ML*M(p));
    for k = 1:NS
        x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tau+Phi(k));
    end;

    m = (0:1:M(p)-1)'; lemda = 0:1:M(p)-1;
    W = exp(1i*(2*pi/M(p)).*kron(m,lemda)); % m*lemda

    bb = zeros(2*(K-M(p))+1,M(p));
    aaa = ones(1,M(p));
    for l = 1:M(p)
        for q = 1:N
            if q ~= p
                    aaa(l) = aaa(l)/sin(pi*M(q)*(taus(p)-taus(q)+(l-1)*T(p))/capT);
                    c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
                    s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
                    bb(2*M(q)+1,l) = 0.5*(c-1i*s);
                    bb(1,l) = conj(bb(2*M(q)+1,l));
            end;
        end;
    end;
    A = diag(aaa); % display(A);
    B = bb; % display(B);

    y1 = upsample(x1,K);
    y1 = reshape(y1,M(p),length(y1)/M(p));

    if M(p)>1
        y1(2:end,:) = flipud(y1(2:end,:));
        for i = 1:M(p)-1
            y1(i+1,:) = filter([0,1],1,y1(i+1,:));
        end;
    end;

    dim = K-M(p); w = -dim:1:dim;

    xlemda = zeros(M(p),size(y1,2));
    for lemda = 0:M(p)-1

        rP = (lemda/M(p))+(0:1:(2*K-1))';
        Fshift = exp(1i*(pi/K).*kron(rP,w));   % r*w

%         h = sinc((n*TQ/T(p))+(lemda/K)-(taus(p)/T(p))).*conv(sinc((lemda/M(p))-(taus(p)/TQ)), kaiser(LF,3),'same');
        h = sinc((n*TQ/T(p))+(lemda/K)-(taus(p)/T(p))).*kaiser_mine1(LF,3,(lemda/M(p))-(taus(p)/TQ));
%        h = sinc((n*TQ/T(p))+(lemda/K)-(taus(p)/T(p))).*knab(LF,3,(lemda/M(p))-(taus(p)/TQ)).';
        y2 = Fshift*B*A*W*W(:,lemda+1)*y1(lemda+1,:);
        for i=1:2*K
            h1 = upsample(downsample(h,2*K,i-1),2*K);
            y2(i,:) = filter(h1,1,y2(i,:));
            y2(i,:) = filter([zeros(1,i-1),1],1,y2(i,:));%,zeros(1,2*K-i)
        end;
        xlemda(lemda+1,:) = sum(y2,1);
    end;
    xp(p,:) = sum(xlemda,1)/M(p);
    clear bb;
end;
y = real(sum(xp,1));
delayP = (length(h)-1)/2;
y = y(1+delayP:end);
x = input(1:end-delayP);
y = y(160:end-60);
x = x(160:end-60);
serP(tt) = serP(tt)+20*log10(norm(x,2)/norm(y-x,2));

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Filterbank Reconstruction of Bandlimited Signals from Nonuniform and
% % Generalized Samples
% % Authors: Y C Eldar and A V Oppenheim
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

y = zeros(N,ML*K);
for p = 1:N
    tau = taus(p)+(0:ML*M(p)-1)*T(p);
    x1 = zeros(1,ML*M(p));
    for k = 1:NS
        x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tau+Phi(k));
    end;
    y1 = upsample(x1,K);

    LFE = M(p)*lcm(M(1),M(2))*2*K+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
    nE = -(LFE-1)/2:1:(LFE-1)/2;
    h = sinc((nE/K)-(taus(p)/T(p))).*kaiser_mine1(LFE,3,-K*(taus(p)/T(p)));

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     bb = zeros(2*(K-M(p))+1,M(p));
%     aaa = ones(1,M(p));
%     for l = 1:M(p)
%         for q = 1:N
%             if q ~= p
%                 aaa(l) = aaa(l)/sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
%                 c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
%                 s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
%                 bb(2*M(q)+1,l) = 0.5*(c-1i*s);
%                 bb(1,l) = conj(bb(2*M(q)+1,l));
%             end;
%         end;
%     end;
%     AE = diag(aaa);
%     BE = bb.';
%
%     dim = K-M(p); w = -dim:1:dim;
%     FE = exp(1i*(pi/(K*M(p))).*kron(nE,w'));
%
%     E1E = exp(1i*(2*pi/M(p)).*kron((0:M(p)-1),(0:M(p)-1)'))/M(p);
%
%     E2E = exp(1i*(2*pi/M(p)).*kron(nE,(0:M(p)-1)'));
%
%     temp = E1E*AE*BE*FE;
%     bbn = h.*sum(E2E.*temp,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 2 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     bb = zeros(2*(K-M(p))+1,M(p));
%     aaa = ones(1,M(p));
%     bbvl = zeros(M(p),LFE);
%     for l = 1:M(p)
%         for q = 1:N
%             if q ~= p
%                 aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
%                 c = sin(pi*M(q)*(-taus(q)+(l-1)*T(p))/capT);
%                 s = cos(pi*M(q)*(taus(q)-(l-1)*T(p))/capT);
%                 bb(2*M(q)+1,l) = 0.5*(c-1i*s);
%                 bb(1,l) = conj(bb(2*M(q)+1,l));
%             end;
%         end;
%         bbv = zeros(2*(K-M(p))+1,LFE);
%         for v = -(K-M(p)):K-M(p)
%             bbv(K-M(p)+1+v,:) = bb(K-M(p)+1+v,l)*exp(1i*(pi/(K*M(p)))*v*nE);
%         end;
%         bbvl(l,:) = sum(bbv,1)/aaa(l);
%     end;
%
%     bbb = zeros(M(p),LFE);
%     bbn = zeros(M(p),LFE);
%     for m = 1:M(p)
%         for l = 1:M(p)
%             bbb(l,:) = bbvl(l,:)*exp(1i*(2*pi/M(p))*(m-1)*(l-1));
%         end
%         bbb = sum(bbb,1);
%         bbn(m,:) = bbb.*exp(1i*(2*pi/M(p))*(m-1)*nE);
%     end;
%     bbn = sum(bbn,1);
%     bbn = bbn.*h/M(p);

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 3 %%%%%%%%%%%%%%%%%%%%%%%%%%%
    aaa = ones(1,M(p));
    bb = ones(M(p),LFE);
    for l = 1:M(p)
        for q = 1:N
            if q ~= p
                aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
                bb(l,:) = bb(l,:).*sin(pi*((nE*TQ/M(p))-taus(q)+(l-1)*T(p))/T(q));
            end;
        end;
        bb(l,:) = bb(l,:)/aaa(l);
    end;

    bbb = zeros(M(p),LFE);
    bbn = zeros(M(p),LFE);
    for m = 1:M(p)
        for l = 1:M(p)
            bbb(l,:) = bb(l,:)*exp(1i*(2*pi/M(p))*(m-1)*(l-1));
        end
        bbb = sum(bbb,1);
        bbn(m,:) = bbb.*exp(1i*(2*pi/M(p))*(m-1)*nE);
    end;
    bbn = sum(bbn,1);
    bbn = bbn.*h/M(p);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    y1 = conv(y1,bbn);
    delay = (length(h)-1)/2;
    y(p,:) = y1(1+delay:M(p):end-delay);
end;
y = sum(real(y),1);
x = input;
y = y(160:end-60);
x = x(160:end-60);
serE(tt) = serE(tt)+20*log10(norm(x,2)/norm(y-x,2));

% % figure();
% subplot(2,1,1);
% plot(([x' y']));
% title('input / output signals');
% xlabel('sample');
% ylabel('signal value');
% grid on;
% subplot(2,1,2);
% plot((x'-y'));
% xlabel('time (sample)');
% ylabel('error value');
% grid on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A realization of Digital Filter Banks for Reconstruction of Uniformly
% sampled signals from nonuniform samples
% Authors: Itami, Watanabe, Nishihara
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x11 = zeros(K,ML);
for k = 1:NS
    x11 = x11 + Amp(k)*sin(2*pi*Frq(k)*tauI+Phi(k));
end;
y1=upsample(x11.',K).';

y = zeros(K,size(y1,2));
for r = 1:K
    y2 = F*b(:,r)*a(r)*y1(r,:);
%     h = sinc((n/K)-tausI(r)/capT).*conv(sinc(-tausI(r)/TQ), kaiser(LF,3),'same');
    h = sinc((n/K)-tausI(r)/capT).*kaiser_mine1(LF,3,-tausI(r)/TQ);
%     h = sinc((n/K)-tausI(r)/capT).*knab(LF,3,-tausI(r)/TQ).';
    for i=1:2*K
            h1 = upsample(downsample(h,2*K,i-1),2*K);
            y2(i,:) = filter(h1,1,y2(i,:));
            y2(i,:) = filter([zeros(1,i-1),1],1,y2(i,:));%,zeros(1,2*K-i)
    end;
    y(r,:) = sum(y2,1);
end;
y = (sum(y,1));
delayI = (length(n)-1)/2;
x=input(1:end-delayI);
y=y(1+delayI:end);
y = y(160:end-60);
x = x(160:end-60);
serI(tt) = serI(tt)+20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Without any reconstruction, the SNR value calculation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wo_reconst = reshape(x11,1,[]);
serNO(tt) = serNO(tt)+20*log10(norm(input(160:end-60),2)/...
            norm(wo_reconst(160:end-60)-input(160:end-60),2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Nonuniformly Sampled Band-Limited Signals
% Using a Differentiator-Multiplier Cascade
% Authors: Stefan Tertinek and Christian Vogel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x1 = reshape(x11,1,K*size(x1,2));
% x1 = x11;% LF = (0:11); Fs = 1;
% Differentiator Design
% figure();
% NFFT = 2^nextpow2(length(Hd)); % Next power of 2 from length of Hd
% HD = fftshift(fft(Hd,NFFT))/length(Hd);
% f = Fs*linspace(-1,1,NFFT);
% % Plot double-sided amplitude spectrum.
% plot(f,2*abs(HD(1:NFFT)))
% title('Double-Sided Amplitude Spectrum of Hd(n)')
% xlabel('Frequency (Hz)')
% ylabel('|HD(f)|')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y1 = filter(Hd,1,x1);
x1 = filter([zeros(1,delayV),1],1,x1);
r2 = filter([zeros(1,delayV),1],1,rr);
e = y1.*r2;
y1 = x1-e;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y2 = filter(Hd,1,y1);
temp = filter([zeros(1,delayV),1],1,y2);
x1 = filter([zeros(1,2*delayV),1],1,x1);
r2 = filter([zeros(1,2*delayV),1],1,r2);
e1 = temp.*r2;
y2 = filter(Hd,1,y2);
e2 = 0.5*y2.*r2.^2;
y2 = x1-e1-e2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y3 = filter(Hd,1,y2);
temp = filter([zeros(1,2*delayV),1],1,y3);
x1 = filter([zeros(1,3*delayV),1],1,x1);
r2 = filter([zeros(1,3*delayV),1],1,r2);
e1 = temp.*r2;
y3 = filter(Hd,1,y3);
temp = filter([zeros(1,delayV),1],1,y3);
e2 = 0.5*temp.*r2.^2;
y3 = filter(Hd,1,y3);
e3 = (y3.*r2.^3)/6;
y3 = x1-e1-e2-e3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y = real(y3(1+6*delayV:end));
x = input(1:end-6*delayV);
y = y(160:end-60);
x = x(160:end-60);
serV(tt) = serV(tt)+20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Band-Limited Periodic Nonuniformly Sampled Signals
% Through Multirate Filter Banks
% Ryan S Prendergast, Bernard C Levy, Paul J Hurst
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x1 = zeros(K,ML);
for k = 1:NS
    x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tauPr+Phi(k));
end;

yb = upsample(x1.',K).';
for i = 1:K
    yb(i,:) = filter(FR(i,:),1,yb(i,:));
end;
y = sum(yb,1);
delayPr = (size(FR,2))/2+K-1;
x=input(1:end-delayPr);
y=y(1+delayPr:end);
y = y(160:end-60);
x = x(160:end-60);
serPr(tt) = serPr(tt)+20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Periodically Nonuniformly Sampled Bandlimited Signals
% Using Time-Varying FIR Filters
% Authors: H. Johansson and Per Lowenborg
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x1 = reshape(x11,1,size(x11,2)*K);
y1 = zeros(K,length(x1));
for j=1:K
    y1(j,:) = filter(hJ(j,:),1,x1);
    y1(j,:) = upsample(downsample(y1(j,:),K,j-1),K)/K;
    y1(j,:) = filter([zeros(1,j-1),1],1,y1(j,:));
end;
y = K*0.25*sum(y1,1);
delayJ = (size(hJ,2)-1)/2;
y = real(y(1+delayJ:end));
x = input(1:end-delayJ);
y = y(160:end-60);
x = x(160:end-60);
serJ(tt) = serJ(tt)+20*log10(norm(x,2)/norm(y-x,2));
end
end
end
serP = serP/(MCruns*(MCruns1-aa));
serE = serE/(MCruns*(MCruns1-aa));
serI = serI/(MCruns*(MCruns1-aa));
serV = serV/(MCruns*(MCruns1-aa));
serPr = serPr/(MCruns*(MCruns1-aa));
serJ = serJ/(MCruns*(MCruns1-aa));
serNO = serNO/(MCruns*(MCruns1-aa));

figure();hold all;
% plot(std,serJ,'kp-','LineWidth',2);
% plot(std,serPr,'ko-','LineWidth',2);
% plot(std,serV,'ks-','LineWidth',2);
% plot(std,serP,'kd-','LineWidth',2);
% plot(std,serI,'k>-','LineWidth',2);
% plot(std,serE,'k+-','LineWidth',2);
% plot(std,serNO,'k+-','LineWidth',2);
plot(std,serJ);
plot(std,serPr);
plot(std,serV);
plot(std,serP);
plot(std,serI);
plot(std,serE);
plot(std,serNO);
% legend('Johansson','Prendergast','Tertinek','Proposed','Itami','Eldar');
legend('Johansson','Prendergast','Tertinek','Proposed','Itami','Eldar', 'W/O Reconst');
xlabel('Standard Deviation (\sigma)','fontsize',14,'fontweight','b');
ylabel('SNR in dB','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

% display('Proposed Method');
% display(sprintf('Delay imposed due to reconstruction system = %d', delayP));
% display(sprintf('Length of prototype filter = %d\n', length(n)));
%
% display('Itami Method');
% display(sprintf('Delay imposed due to reconstruction system = %d', delayI));
% display(sprintf('Length of prototype filter = %d', length(n)));
% display(sprintf('Length of fractional delay filter = %d\n', length(n)));
%
% display('Vogel Method');
% display(sprintf('Delay imposed due to reconstruction system = %d', 6*delayV));
% display(sprintf('Length of differentiator = %d\n', LF));
%
% display('Prendergast Method');
% display(sprintf('Delay imposed due to reconstruction system = %d', delayPr));
% display(sprintf('Length of prototype filter = %d', size(FR,2)));
% display(sprintf('Length of fractional delay filter = %d\n', length(n)));
%
% display('Johansson Method');
% display(sprintf('Delay imposed due to reconstruction system = %d', delayJ));
% display(sprintf('Length of prototype filter = %d\n', size(h,2)));

% figure();
% subplot(2,1,1);
% plot(([x' y']));
% title('input / output signals');
% xlabel('sample');
% ylabel('signal value');
% grid on;
% subplot(2,1,2);
% plot((x'-y'));
% xlabel('time (sample)');
% ylabel('error value');
% grid on;


function [BCMatrix, BCRHS] = boundaryConditionCylindrical3D(BC)
% It creates the matrix of coefficient based on the BC structureprovided
% by the user. It also generates the right hand side vector of the linear
% system of equations
%
% SYNOPSIS:
%       [BCMatrix, BCRHS] = boundaryConditionCylindrical3D(BC)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Note: I use a for loop here fr more readability of the code!

% extract data from the mesh structure
Nxyz = BC.domain.dims;
Nx = Nxyz(1); Ntetta = Nxyz(2); Nz = Nxyz(3);
G=reshape(1:(Nx+2)*(Ntetta+2)*(Nz+2), Nx+2, Ntetta+2, Nz+2);
dx_1 = BC.domain.cellsize.x(1);
dx_end = BC.domain.cellsize.x(end);
dtetta_1 = BC.domain.cellsize.y(1);
dtetta_end = BC.domain.cellsize.y(end);
dz_1 = BC.domain.cellsize.z(1);
dz_end = BC.domain.cellsize.z(end);
rp = repmat(BC.domain.cellcenters.x, 1, Nz);

% number of boundary nodes (axact number is 2[(m+1)(n+1)*(n+1)*(p+1)+(m+1)*p+1]:
nb = 8*((Nx+1)*(Ntetta+1)+(Nx+1)*(Nz+1)+(Ntetta+1)*(Nz+1));

% define the vectors to be used for the creation of the sparse matrix
ii = zeros(nb,1);
jj = zeros(nb,1);
s = zeros(nb,1);

% define the RHS column vector
BCRHS = zeros((Nx+2)*(Ntetta+2)*(Nz+2), 1);

% assign value to the corner nodes (useless cells)
q = 1:8;
ii(q) = BC.domain.corners; jj(q) = BC.domain.corners;
s(q) = 1; BCRHS(BC.domain.corners) = 0;

% assign values to the edges (useless cells)
q = q(end)+(1:length(BC.domain.edges));
ii(q) = BC.domain.edges; jj(q) = BC.domain.edges;
s(q) = 1; BCRHS(BC.domain.edges) = 0;

% Assign values to the boundary condition matrix and the RHS vector based
% on the BC structure
if (BC.top.periodic ==0) && (BC.bottom.periodic == 0)
    % top boundary
    j=Ntetta+2;
    i=2:Nx+1;
    k=2:Nz+1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = BC.top.b/2 + BC.top.a./(dtetta_end*rp);
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j-1,k); s(q) = BC.top.b/2 - BC.top.a./(dtetta_end*rp);
    BCRHS(G(i,j,k)) = BC.top.c;

    % Bottom boundary
    j=1;
    i=2:Nx+1;
    k=2:Nz+1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j+1,k);  s(q) = -(BC.bottom.b/2 + BC.bottom.a./(dtetta_1*rp)); % consider the reverse direction of normal
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k); s(q) = -(BC.bottom.b/2 - BC.bottom.a./(dtetta_1*rp)); % consider the reverse direction of normal
    BCRHS(G(i,j,k)) = -(BC.bottom.c);
elseif (BC.top.periodic ==1) || (BC.bottom.periodic == 1) % periodic
    % top boundary
    j=Ntetta+2;
    i=2:Nx+1;
    k=2:Nz+1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j-1,k);  s(q) = -1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,1,k); s(q) = dtetta_end/dtetta_1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,2,k); s(q) = -dtetta_end/dtetta_1;
    BCRHS(G(i,j,k)) = 0;

    % Bottom boundary
    j=1;
    i=2:Nx+1;
    k=2:Nz+1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j+1,k);  s(q) = 1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,Ntetta+1,k); s(q) = -1;
    q = q(end)+(1:Nx*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,Ntetta+2,k); s(q) = -1;
    BCRHS(G(i,j,k)) = 0;
end

if (BC.right.periodic == 0) && (BC.left.periodic == 0)
    % Right boundary
    i=Nx+2;
    j=2:Ntetta+1;
    k=2:Nz+1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = BC.right.b/2 + BC.right.a/dx_end;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i-1,j,k); s(q) = BC.right.b/2 - BC.right.a/dx_end;
    BCRHS(G(i,j,k)) = BC.right.c;

    % Left boundary
    i = 1;
    j=2:Ntetta+1;
    k=2:Nz+1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i+1,j,k);  s(q) = -(BC.left.b/2 + BC.left.a/dx_1); % consider the reverse direction of normal
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k); s(q) = -(BC.left.b/2 - BC.left.a/dx_1); % consider the reverse direction of normal
    BCRHS(G(i,j,k)) = -(BC.left.c);
elseif (BC.right.periodic == 1) || (BC.left.periodic == 1) % periodic
    % Right boundary
    i=Nx+2;
    j=2:Ntetta+1;
    k=2:Nz+1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i-1,j,k);  s(q) = -1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(1,j,k); s(q) = dx_end/dx_1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(2,j,k); s(q) = -dx_end/dx_1;
    BCRHS(G(i,j,k)) = 0;

    % Left boundary
    i = 1;
    j=2:Ntetta+1;
    k=2:Nz+1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(i+1,j,k);  s(q) = 1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(Nx+1,j,k); s(q) = -1;
    q = q(end)+(1:Ntetta*Nz);
    ii(q) = G(i,j,k);  jj(q) = G(Nx+2,j,k); s(q) = -1;
    BCRHS(G(i,j,k)) = 0;
end

if (BC.front.periodic == 0) && (BC.back.periodic == 0)
    % Front boundary
    k=Nz+2;
    i = 2:Nx+1;
    j=2:Ntetta+1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = BC.front.b/2 + BC.front.a/dz_end;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k-1); s(q) = BC.front.b/2 - BC.front.a/dz_end;
    BCRHS(G(i,j,k)) = BC.front.c;

    % Back boundary
    k=1;
    i = 2:Nx+1;
    j=2:Ntetta+1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k+1);  s(q) = -(BC.back.b/2 + BC.back.a/dz_1); % consider the reverse direction of normal
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k); s(q) = -(BC.back.b/2 - BC.back.a/dz_1); % consider the reverse direction of normal
    BCRHS(G(i,j,k)) = -(BC.back.c);
elseif (BC.front.periodic == 1) || (BC.back.periodic == 1) % periodic
    % Front boundary
    k=Nz+2;
    i = 2:Nx+1;
    j=2:Ntetta+1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k-1);  s(q) = -1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,1); s(q) = dz_end/dz_1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,2); s(q) = -dz_end/dz_1;
    BCRHS(G(i,j,k)) = 0;

    % Back boundary
    k=1;
    i = 2:Nx+1;
    j=2:Ntetta+1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k);  s(q) = 1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,k+1);  s(q) = 1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,Nz+1); s(q) = -1;
    q = q(end)+(1:Nx*Ntetta);
    ii(q) = G(i,j,k);  jj(q) = G(i,j,Nz+2); s(q) = -1;
    BCRHS(G(i,j,k)) = 0;
end

% Build the sparse matrix of the boundary conditions
BCMatrix = sparse(ii(1:q(end)), jj(1:q(end)), s(1:q(end)), ...
    (Nx+2)*(Ntetta+2)*(Nz+2), (Nx+2)*(Ntetta+2)*(Nz+2));


function coeffS = computeMcoeffs(coeffS,symmetric)
% function coeffS = computeMcoeffs(coeffS,symmetric)
%
% APA, 6/11/2018

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

%% ComputeRemainingCoefficients(bool symmetric)

if symmetric

    coeffS.M1 = coeffS.N1 - coeffS.D1 * coeffS.N0;
    coeffS.M2 = coeffS.N2 - coeffS.D2 * coeffS.N0;
    coeffS.M3 = coeffS.N3 - coeffS.D3 * coeffS.N0;
    coeffS.M4 = -coeffS.D4 * coeffS.N0;

else

   coeffS.M1 = -( coeffS.N1 - coeffS.D1 * coeffS.N0 );
   coeffS.M2 = -( coeffS.N2 - coeffS.D2 * coeffS.N0 );
   coeffS.M3 = -( coeffS.N3 - coeffS.D3 * coeffS.N0 );
   coeffS.M4 = coeffS.D4 * coeffS.N0;
end

% Compute coefficients to be used at the boundaries ...
% in order to simulate edge extension boundary conditions.
coeffS.SN = coeffS.N0 + coeffS.N1 + coeffS.N2 + coeffS.N3;
coeffS.SM = coeffS.M1 + coeffS.M2 + coeffS.M3 + coeffS.M4;
coeffS.SD = 1.0 + coeffS.D1 + coeffS.D2 + coeffS.D3 + coeffS.D4;

coeffS.BN1 = coeffS.D1 * coeffS.SN / coeffS.SD;
coeffS.BN2 = coeffS.D2 * coeffS.SN / coeffS.SD;
coeffS.BN3 = coeffS.D3 * coeffS.SN / coeffS.SD;
coeffS.BN4 = coeffS.D4 * coeffS.SN / coeffS.SD;

coeffS.BM1 = coeffS.D1 * coeffS.SM / coeffS.SD;
coeffS.BM2 = coeffS.D2 * coeffS.SM / coeffS.SD;
coeffS.BM3 = coeffS.D3 * coeffS.SM / coeffS.SD;
coeffS.BM4 = coeffS.D4 * coeffS.SM / coeffS.SD;


function x = prox_sum_square(v, lambda)
% PROX_SUM_SQUARE    Proximal operator of sum-of-squares.
%
%   prox_sum_square(v,lambda) is the proximal operator of
%   (1/2)||.||_2^2 with parameter lambda.

    x = (1/(1 + lambda))*v;
end


function [aw,tt1, tt2, tmc, mag_zone]=bvalue(this, mcType, method)
    %BVALUE evaluate b-value, a-value and magnitude of completeness
    % of an earthquake catalog stored in a Catalog object.
    %
    % BVALUE(COBJ, MCTYPE) produces a Gutenberg-Richter type plot
    %    with the best fit line and display of b-,a-values and Mc
    %    for the catalog object COBJ. MCTYPE is a number from 1-5
    %    to select the algorithm used for calculation of the
    %    magnitude of completeness. Options are:
    %
    %    1: Maximum curvature
    %    2: Fixed Mc = minimum magnitude (Mmin)
    %    3: Mc90 (90% probability)
    %    4: Mc95 (95% probability)
    %    5: Best combination (Mc95 - Mc90 - maximum curvature)

    % Liberally adapted from original code in ZMAP.
    % Author: Silvio De Angelis, 27/07/2012 00:00:00
    % Modified and included in Catalog by Glenn Thompson,
    % 14/06/2014

    % This program is free software; you can redistribute it and/or modify
    % it under the terms of the GNU General Public License cobj.magas 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 should have received a copy of the GNU General Pucobj.magblic License
    % along with this program; if not, write to the
    % Free Software Foundation, Inc.,
    % 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

    if nargin < 2
        disp('--------------------------------------------------------')
        disp('ERROR: Usage is: bvalue(cobj, mcType). mcType not specified')
        disp('--------------------------------------------------------')
        disp('mcType can be:')
        disp('1: Maximum curvature')
        disp('2: Fixed Mc = minimum magnitude (Mmin)')
        disp('3: Mc90 (90% probability)')
        disp('4: Mc95 (95% probability)')
        disp('5: Best combination (Mc95 - Mc90 - maximum curvature)')
        return
    end

    % form magnitude vector - removing any NaN values with find
    good_magnitude_indices = find(this.data > 0.0);
    if strcmp(method, 'power')
        mag = log10(this.data(good_magnitude_indices));
    elseif strcmp(method, 'exponential')
        mag = this.data(good_magnitude_indices);
    end

    %MIN AND MAX MAGNITUDE IN CATALOG
    minimum_mag = min(mag);
    maximum_mag = max(mag);

    %COUNT EVENTS IN EACH MAGNITUDE BIN
    if strcmp(method, 'power')
        magrange = minimum_mag:0.1:maximum_mag;
    elseif strcmp(method, 'exponential')
        magrange = 10.^(log10(minimum_mag):0.1:log10(maximum_mag));
    end
    [bval, xt2] = hist(mag, magrange);

    %CUMULATIVE NUMBER OF EVENTS IN EACH MAGNITUDE BIN
    bvalsum = cumsum(bval);

    %NUMBER OF EVENTS IN EACH BIN IN REVERSE ORDER
    bval2 = bval(length(bval):-1:1);

    %NUMBER OF EVENTS IN EACH MAGNITUDE BIN IN REVERSE ORDER
    bvalsum3 = cumsum(bval(length(bval):-1:1));

    %BINS IN REVERSE ORDER
    xt3 = fliplr(magrange);
    backg_ab = log10(bvalsum3);

    %CREATE FIGURE WINDOW AND MAKE FREQUENCY-MAGNITUDE PLOT
    figure('Color','w','Position',[0 0 600 600])

    pl = semilogy(xt3,bvalsum3,'sb');

    set(pl, 'LineWidth', [1.0],'MarkerSize', [10],'MarkerFaceColor','r','MarkerEdgeColor','k');
    axis square
    hold on

    %pl1 = semilogy(xt3,bval2,'^b');
    %set(pl1, 'LineWidth',[1.0],'MarkerSize',[10],'MarkerFaceColor','w','MarkerEdgeColor','k');
    if strcmp(method, 'power')
        %xlabel('Log_1_0(Amplitude)','Fontsize', 12)
        xlabel('Magnitude','Fontsize', 12)
    elseif strcmp(method, 'exponential')
        xlabel('Amplitude','Fontsize', 12)
    end

    ylabel('Cumulative Minutes','Fontsize',12)
    set(gca,'visible','on','FontSize',12,'FontWeight','normal',...
        'FontWeight','bold','LineWidth',[1.0],'TickDir','in','Ticklength',[0.01 0.01],...
        'Box','on','Tag','cufi','color','w')

    %ESTIMATE B-VALUE (MAX LIKELIHOOD ESTIMATE)
    Nmin = 10;
    fMccorr = 0;
    fBinning = 0.1;

    if length(mag) >= Nmin

        %GOODNESS-OF-FIT TO POWER LAW
        %%%%%%%%%%%%%%%%%% mcperc_ca3.m start %%%%%%%%%%%%%%%%%%%%
        % This is a completeness determination test

        if strcmp(method, 'power')
            [bval,xt2] = hist(mag,-2:0.1:6);
        elseif strcmp(method, 'exponential')
            [bval,xt2] = hist(log10(mag),-2:0.1:6);
        end
        l = max(find(bval == max(bval)));
        magco0 =  xt2(l)

        dat = [];

        %for i = magco0-0.6:0.1:magco0+0.2
        for i = magco0-0.5:0.1:magco0+0.7
            if strcmp(method, 'power')
                l = mag >= i - 0.0499;
            elseif strcmp(method, 'exponential')
                l = mag >= 10^(i - 0.0499);
            end
            nu = length(mag(l));
            if length(mag(l)) >= 25;
                %[bv magco stan av] =  bvalca3(catZmap(l,:),2,2);
                if strcmp(method, 'power')
                    [mw bv2 stan2 av] =  bvalue_lib.bmemag(mag(l));
                elseif strcmp(method, 'exponential')
                    [mw bv2 stan2 av] =  bvalue_lib.bmemag(log10(mag(l)));
                end
                bvalue_lib.synthb_aut;
                dat = [ dat ; i res2];
            else
                dat = [ dat ; i nan];
            end

        end

        j =  min(find(dat(:,2) < 10 ));
        if isempty(j) == 1; Mc90 = nan ;
        else;
            Mc90 = dat(j,1);
        end

        j =  min(find(dat(:,2) < 5 ));
        if isempty(j) == 1; Mc95 = nan ;
        else;
            Mc95 = dat(j,1);
        end

        j =  min(find(dat(:,2) < 10 ));
        if isempty(j) == 1; j =  min(find(dat(:,2) < 15 )); end
        if isempty(j) == 1; j =  min(find(dat(:,2) < 20 )); end
        if isempty(j) == 1; j =  min(find(dat(:,2) < 25 )); end
        j2 =  min(find(dat(:,2) == min(dat(:,2)) ));
        %j = min([j j2]);

        Mc = dat(j,1);
        magco = Mc;
        prf = 100 - dat(j2,2);
        if isempty(magco) == 1; magco = nan; prf = 100 -min(dat(:,2)); end
        %display(['Completeness Mc: ' num2str(Mc) ]);
        %%%%%%%%%%%%%%%%%% mcperc_ca3.m end %%%%%%%%%%%%%%%%%%%%%%

        %CALCULATE MC
        [fMc] = bvalue_lib.calc_Mc(mag, mcType, fBinning, fMccorr);
        l = mag >= fMc-(fBinning/2);
        if length(mag(l)) >= Nmin
            [fMeanMag, fBValue, fStd_B, fAValue] =  bvalue_lib.calc_bmemag(mag(l), fBinning);
        else
            [fMc, fBValue, fStd_B, fAValue] = deal(NaN);
        end

        %STANDARD DEV OF a-value SET TO NAN;
        [fStd_A, fStd_Mc] = deal(NaN);

    else
        [fMc, fStd_Mc, fBValue, fStd_B, fAValue, fStd_A, ...
            fStdDevB, fStdDevMc] = deal(NaN);
    end

    magco = fMc; % magnitude of completeness?
    index_low=find(xt3 < magco+.05 & xt3 > magco-.05);
    mag_hi = xt3(1);
    index_hi = 1;
    mz = xt3 <= mag_hi & xt3 >= magco-.0001;
    mag_zone=xt3(mz);
    y = backg_ab(mz);

    %PLOT MC IN FIGURE
    Mc = semilogy(xt3(index_low),bvalsum3(index_low)*1.5,'vk');
    set(Mc,'LineWidth',[1.0],'MarkerSize',7)
    Mc = text(xt3(index_low)+0.2,bvalsum3(index_low)*1.5,'Mc');
    set(Mc,'FontWeight','normal','FontSize',12,'Color','k')

    %CREATE AND PLOT FIT LINE
    sol_type = 'Maximum Likelihood Solution';
    bw=fBValue;
    aw=fAValue;
    ew=fStd_B;
    p = [ -1*bw aw];
    f = polyval(p,mag_zone);
    f = 10.^f;

    hold on
    ttm= semilogy(mag_zone,f,'k');
    set(ttm,'LineWidth',[2.0])
    std_backg = ew;

    %ERROR CALCULATIONS
    %b = mag;
    bv = [];
    si = [];

    set(gca,'XLim',[min(mag)-0.5  max(mag+0.5)])
    %set(gca,'YLim',[0.9 length(mag+30)*2.5]);

    p=-p(1,1);
    p=fix(100*p)/100;
    tt1=num2str(bw,3);
    tt2=num2str(std_backg,1);
    tt4=num2str(bv,3);
    tt5=num2str(si,2);
    tmc=num2str(magco,2);
    rect=[0 0 1 1];
    h2=axes('position',rect);
    set(h2,'visible','off');
    a0 = aw-log10((max(this.dnum)-min(this.dnum))/365);

    text(.53,.88, ['b-value = ',tt1,' +/- ',tt2,',  a value = ',num2str(aw,3)],'FontSize',12);
    %text(.53,.85,sol_type,'FontSize',12 );
    text(.53,.82,['Magnitude of Completeness = ',tmc],'FontSize',12);

    % Glenn 20150111 add R^2 value
    thiscorr = corrcoef(mag_zone, f)
    r2 = thiscorr(1,2);
    thiscorr2 = corrcoef(mag_zone, log10(f))
    r22 = thiscorr2(1,2);
    %text(.53,.76,['R^2 = ',num2str(r2)],'FontSize',12);
    %text(.53,.70,['R^2 = ',num2str(r22)],'FontSize',12);

end


function h = conv(f, g)
%CONV   Convolution of BNDFUN objects.
%   H = CONV(F, G) produces the convolution of BNDFUN objects F and G:
%                     -
%                    /
%           H(x) =   |    F(t) G(x-t) dt,  x in [a + c, b + d]
%                    /
%                   -
%   where domain(F) is [a, b] and domain(G) is [c, d]. The integral is taken
%   over all t for which the integrand is defined: max(a, x - d) <= t <= min(b,
%   x - c).  The breakpoints of H are all pairwise sums of the breakpoints of F
%   and G.
%
%   Note that CONV only supports piecewise-smooth functions on bounded domains.
%
% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.
%
% Nick Hale and Alex Townsend, 2014

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DEVELOPER NOTE:
%
% For further details, see Hale and Townsend, "An algorithm for the convolution
% of Legendre series", SIAM Journal on Scientific Computing, Vol. 36, No. 3,
% pages A1207-A1220, 2014.
%
% In the following, it is assumed that the length of the domain of g is greater
% than the length of the domain of f. If this is not the case, then simply
% compute h = conv(g, f), which is equivalent. We assume f and g are polynomials
% of degree M and N, resepectively. If f and g are piecewise-defined, one can
% use the bilinearity of convolution and convolve each each the FUNs
% individually.
%
% The general convolution domain (for smooth functions) is as follows:
%           .___________________________
%          /|                  |      /
%        /  |                  |    /
%      /    |                  |  /
%    /______|__________________|/
%  a+c     b+c                a+d     b+d
%
% The triangular pieces at end are dealt with using a convolution theorem for
% Legendre polynomials, which leads to a convenient recurrence relation. The
% cost of this is O(m*n) and results in a polynomial of degree m+n. See
% EASYCONV() for details.
%
% Similarly one can show the interior rectangle results in a polynomial of
% degree n. This can be computed by patching with R = floor[(d-c) / (b-a)]
% parallelograms (although triangle Z is not used). Each parallelogram requires
% restricting g to a suitable subdomain, which costs O(n^2) operations. Total
% complexity ratio*(m*n + n*n)
%            ___________________________
%          /       /       /:     /   /
%        /       /       /  : Z /   /          <-- R patches
%      /       /       /    : /   /
%    /_______/_______/______/__ /
%  a+c     b+c             fl  a+d     b+d
%
% The final piece is computed via further parallelogram subdivision starting
% from the right (B), and a smaller subdivision in both f and g (C).
% Contributions from D and E are discarded, as they have already been counted
% above. Complexity O(m^2 + n^2)
%   ________________
%   : /E|C/:      /
%   :   /  :    /
%   : /D|B :  /
%   /___|__:/
%  a+c fl a+d     b+d
%
% Rather than make a BNDFUN corresponding to the each of the patches, we
% instead evaluate directly on a corresonding Chebyshev grid of appropriate
% size, which turns out to be far more efficient.
%
% Total complexity: O( R*(m*n + n*n) + m*m )
%
% However, in the case when R is too big we revert to a Clenshaw-Curtis
% quadrature-based approach to compute the convolution in the inner rectangle.
% This approach has a complexity O( (m + n)^3 ). We naively compare this with
% the big-O term above to determine which approach to use.

% [TODO]: It's possible this should be pushed further to the chebtech level.
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Return empty for an empty input:
if ( isempty(f) || isempty(g) )
    h = bndfun();
    return
end

% Extract the domain:
domF = f.domain;
a = domF(1);
b = domF(2);

domG = g.domain;
c = domG(1);
d = domG(2);

% Ensure that g is the signal (i.e., on the larger domain) and f is the filter:
if ( (b - a) > (d - c) )
    h = conv(g, f);
    return
end

% Useful things:
M = length(f);                               % Length of F
N = length(g);                               % Length of g
numPatches = floor((d - c) / (b - a));       % Number of patches required
x = chebpts(N, [b+c, a+d], 1);               % Chebyshev grid for interior piece
y = 0*x;                                     % Initialise values in interior
map = @(x, a, b) (x-a)/(b-a) - (b-x)/(b-a);  % Map from [a, b] --> [-1, 1]

% If there are too many patches then the HT approach is too slow. In such a case
% we resort to the standard quadrature-based approach (but still use the HT
% approach for the two triangular domains at the ends).
coeffsConvCost = numPatches*((M+N)*N);
quadConvCost = (M+N)^3;
if ( numPatches > 1 && coeffsConvCost > quadConvCost )
    h_left = conv(f, restrict(g, c+[0, b-a]));     % Left triangle
    h_right = conv(f, restrict(g, d-[(b-a) 0]));   % Right tirangle
    % Middle:
    [t, w] = legpts((M+N+5)/2, [a,b]);             % Legendre grid
    [tt, xx] = meshgrid(t,x);                      % Cheb/Leg grid to evaluate g
    ft = feval(f,t);                               % Evaluate f
    gxmt = feval(g, xx-tt);                        %    and g (expensive)
    y = gxmt*(w'.*ft);                             % Compute integral
    y = chebtech1.vals2coeffs(y);                  % Convert values to coeffs
    % Trim small coefficients:
    ay = abs(y); my = max(ay); loc = max(find(ay > 10*eps*my, 1, 'last'),1);
    if ( isempty(loc) ), loc = 1; end              % Deal with case when y = 0
    y = y(1:loc);
    data.domain = [b+c, a+d];
    h_mid = bndfun({[],y}, data);                  % Make h_mid from coeffs
    h = {h_left{1}, h_mid, h_right{end}};          % Combine three pieces
    h = fixMaps(h);                                % Ensure domain ends match
    return
end

% Trim small coefficients in f:
f_cheb = get(f, 'coeffs'); af = abs(f_cheb); mf = max(af);
loc = find(af > 10*eps*mf, 1, 'last'); if ( isempty(loc) ), loc = 1; end
f_cheb = f_cheb(1:loc);
f_leg = cheb2leg(f_cheb);                   % Legendre coefficients of f

% Restrict g:
doms = c + (b - a)*(0:numPatches);
g_restricted = restrict(g, doms);
doms = c + (b - a)*(0:numPatches);           % Subdomains
if ( ~iscell(g_restricted) )
    % If doms happened to be domain(g), restrict would return a cell.
    g_restricted = {g_restricted};
end

% Loop over the patches:
for k = 1:numPatches
                          %         _____
    dk = doms([k, k+1]);  %       /|    /
    dk_left  = a + dk(1); %     /  |  /
    dk_mid   = a + dk(2); %   /____|/
    dk_right = b + dk(2); %  dkl  dkm   dkr
    gk = g_restricted{k};                          % g on this subdomain
    gk = simplify(gk);                             % Simplify for efficiency
    gk_leg = cheb2leg(get(gk, 'coeffs'));          % Legendre coefficients
    [hLegL, hLegR] = easyConv(f_leg, gk_leg);      % Convolution on this domain

    % The left triangle for the kth patch:
    ind = (dk_left <= x) & (x < dk_mid); % Locate the grid values in [dkl, dkr]:
    if ( k == 1 ) % First piece:
        hLegL = leg2cheb(hLegL);                   % Cheb. coeffs of left tri.
        data.domain = [dk_left, dk_mid];
        h_left = bndfun({[], hLegL}, data);        % Make BNDFUN from coeffs
    else          % Subsequent left pieces
        z = map(x(ind), dk_left, dk_mid);          % Map grid points to [-1, 1]
        tmp = clenshawLegendre(z, hLegL);          % Evaluate via recurrence
        y(ind) = y(ind) + tmp;                     % Append
    end

    % The right triangle for the kth patch:
    if ( k < numPatches )                          % Not needed for final patch!
        % Locate the grid values in [dkl, dkr]:
        ind = (dk_mid <= x) & (x < dk_right);
        z = map(x(ind), dk_mid, dk_right);
        tmp = clenshawLegendre(z, hLegR);
        y(ind) = y(ind) + tmp;
    end

end

if ( abs((b-a)-(d-c)) < 10*eps(norm([a b c d], inf)) )
    % If there's only one patch, then we already have all the information reqd.
    hLegR = leg2cheb(hLegR);                        % Cheb coeffs of right tri.
    data.domain = d + [a b];
    h_right = bndfun({[], hLegR}, data);            % Make BNDFUN from coeffs
    h_mid = bndfun();

else
    % Final right right triangle:
    %  ________________
    %  : /E|C/:      /
    %  :   /  : A  /
    %  : /D|B :  /
    %  /___|__:/
    %     fl a+d     b+d

    finishLocation = a + c + numPatches*(b - a);    % Where patches got to. (fl)
    gk = restrict(g, d-[(b-a) 0]);                  % g on appropriate domain
    gk = simplify(gk);                              % Simplify for efficiency
    gk_leg = cheb2leg(get(gk, 'coeffs'));           % Legendre coeffs
    [hLegL, hLegR] = easyConv(f_leg, gk_leg);       % Conv on A and B
    hLegR = leg2cheb(hLegR);                        % Cheb coeffs on A
    data.domain = [d+a, d+b];
    h_right = bndfun({[], hLegR}, data);            % Make BNDFUN from coeffs

    % Remainder piece: (between fl and a+d)
    remainderWidth = d + a - finishLocation; % b+d-fl-(b-a)
    if ( remainderWidth > 0 )
        ind = finishLocation <= x;           % Discard D and E

        % B: (Coeffs were computed above)
        z = map(x(ind), d - b + 2*a, d + a); % Map grid to [-1, 1]
        tmp = clenshawLegendre(z, hLegL);    % Evaluate via recurrence
        y(ind) = tmp;                        % Store

        % C:
        domfk = b + [-remainderWidth, 0];               % Domain of fk
        domfk(1) = max(domfk(1), f.domain(1));          % Ensure domfk is a
        domfk(end) = min(domfk(end), f.domain(end));    %  valid subdomain
        fk = restrict(f, domfk);                        % Restrict f
        fk = simplify(fk);                              % Simplify f
        fk_leg = cheb2leg(get(fk, 'coeffs'));           % Legendre coeffs
        domgk = [finishLocation, d + a] - b;            % Domain of gk
        domgk(1) = max(domgk(1), g.domain(1));          % Ensure domgk is a
        domgk(end) = min(domgk(end), g.domain(end));    %  valid subdomain
        gk = restrict(g, domgk);                        % Restrict g
        gk = simplify(gk);                              % Simplify g
        gk_leg = cheb2leg(get(gk, 'coeffs'));           % Legendre coeffs
        [~, hLegR] = easyConv(fk_leg, gk_leg);          % Conv
        z = map(x(ind), finishLocation, d + a);         % Map to [-1, 1]
        tmp = clenshawLegendre(z, hLegR);               % Eval via recurrence
        y(ind) = y(ind) + tmp*remainderWidth/(b - a);   % Scale and append
    end
    % Convert values to coeffs (we don't want to construct a chebtech1)
    y = chebtech1.vals2coeffs(y);

    % Construct BNDFUN of the interior (rectangle) using coefficients:
    data.domain = [b+c, a+d];
    h_mid = bndfun({[], y}, data);

end

% h_mid can be empty so return the three or two pieces as a cell array:
if ( isempty(h_mid) )
    h = {h_left*(b-a)/2, h_right*(b-a)/2};
else
    h = {h_left*(b-a)/2, h_mid*(b-a)/2, h_right*(b-a)/2};
end
h = fixMaps(h);

end

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

function h = fixMaps(h)
% Ensure endpoints of BNDFUNs for adjacent subintervals match exactly. NB: This
% is a bit inefficient if consecutive endpoints get adjusted, as some BNDFUNs
% may have their maps changed twice, but the map-change operation is fast enough
% that this shouldn't matter.
for n = 2:1:numel(h)
    end_left = h{n-1}.domain(end);
    end_right = h{n}.domain(1);
    if ( end_left ~= end_right )
        hs_left = norm(h{n-1}.domain, Inf);  % hscale for left piece.
        hs_right = norm(h{n}.domain, Inf);   % hscale for right piece.

        % If there's a mismatch, it should be small because the BNDFUNs should
        % come out in order corresponding to consecutive adjacent subintervals.
        if ( abs(end_left - end_right) < 2*eps*max(hs_left, hs_right) )
            new_end = (end_left + end_right)/2;
            h{n-1} = changeMap(h{n-1}, [h{n-1}.domain(1) new_end]);
            h{n} = changeMap(h{n}, [new_end h{n}.domain(end)]);
        else
            % We should only get here if a programmer error made elsewhere
            % causes the BNDFUNs to get out of order.
            error('CHEBFUN:BNDFUN:conv:nonConsecutive', ...
                'Pieces do not belong to consecutive subintervals.');
        end
    end
end

end

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

function [gammaL, gammaR] = easyConv(alpha, beta)
% Convolution using Legendre expansions and the analoguous convolution theorem.
% See Hale and Townsend, "An algorithm for the convolution of Legendre series",
% SIAM Journal on Scientific Computing, Vol. 36, No. 3, pages A1207-A1220, 2014.

% Better computational efficiency is achieved when g has the lower degree:
if ( length(beta) > length(alpha) )
    tmp = alpha;
    alpha = beta;
    beta = tmp;
end

% Maximum degree of result:
MN = length(alpha) + length(beta);

% Pad to make length n + 1.
alpha = [ alpha ; zeros(MN - length(alpha), 1) ];

% S represents multiplication by 1/z in spherical Bessel space:
e = [[1 ; 1./(2*(1:(MN-1)).'+1)], [1 ; zeros(MN-1, 1)], -1./(2*(0:MN-1).'+1)];
S = spdiags(e, -1:1, MN, MN);

gammaL = rec(S, alpha, beta, -1); % Chebyshev coeffs for the left piece
S(1,1) = -1;                      % Update S
gammaR = rec(S, -alpha, beta, 1); % Chebyshev coeffs for the right piece

%%%%%%%%%%%%%%%%%%%%%%%%%%% MATRIX FREE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function gamma = rec(S, alpha, beta, sgn)
        % Compute the Legendre coefficients of the convolution on L/R piece.
        % See Theorem 4.1 of paper.

        % Initialise scl:
        N = length(beta);
        scl = 1./(2*(1:N).'-1);
        scl(2:2:end) = -scl(2:2:end);

        % First column of B:
        vNew = S*alpha;
        v = vNew;
        gamma = beta(1)*vNew;
        beta_scl = scl.*beta;
        beta_scl(1) = 0;
        gamma(1) = gamma(1) + vNew(1:N).'*beta_scl;

        % The scalar case is trivial:
        if ( length(beta) == 1 )
            return
        end

        % Second column of B:
        vNew = S*v + sgn*v;
        vOld = v;
        v = vNew;
        vNew(1) = 0;

        gamma = gamma + beta(2)*vNew;
        beta_scl = -beta_scl*((2 - 0.5)/(2 - 1.5));
        beta_scl(2) = 0;
        gamma(2) = gamma(2) + vNew(1:N).'*beta_scl;

        % Loop over remaining columns using recurrence:
        for n = 3:N
            vNew = (2*n-3) * (S * v) + vOld; % Recurrence
            vNew(1:n-1) = 0;                 % Zero terms
            gamma = gamma + vNew*beta(n);    % Append to g

            % Recurrence is unstable for j < k. Correct for upper-tri part:
            beta_scl = -beta_scl*((n-.5)/(n-1.5));
            beta_scl(n) = 0;
            gamma(n) = gamma(n) + vNew(1:N).'*beta_scl;

            vOld = v;
            v = vNew;

        end

        ag = abs(gamma);
        mg = max(ag);
        loc = find(ag > eps*mg, 1, 'last');
        gamma = gamma(1:loc);

    end

end

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

function val = clenshawLegendre(x, alpha)
% Evaluate a Legendre expansion with coefficient alpha at x.

n = length(alpha);
b_old = 0;
b_cur = 0;
for k = (n-1):-1:1
  b_new = alpha(k+1) + (2*k + 1)/(k + 1)*x.*b_cur - (k + 1)/(k + 2)*b_old;
  b_old = b_cur;
  b_cur = b_new;
end
val = alpha(1) + x.*b_cur - .5*b_old;

end

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


% This is the main extended Kalman filter SLAM loop. This script calls all the required
% functions in the correct order.
%
% You can disable the plotting or change the number of steps the filter
% runs for to ease the debugging. You should however not change the order
% or calls of any of the other lines, as it might break the framework.
%
% If you are unsure about the input and return values of functions you
% should read their documentation which tells you the expected dimensions.

% Turn off pagination:
more off;

% clear all variables and close all windows
clear all;
close all;

% Make tools available
addpath('tools');

% Read world data, i.e. landmarks. The true landmark positions are not given to the robot
landmarks = read_world('../data/world.dat');
% load landmarks;
% Read sensor readings, i.e. odometry and range-bearing sensor
data = read_data('../data/sensor_data.dat');
%load data;

INF = 1000;
% Get the number of landmarks in the map
N = size(landmarks,2);

% observedLandmarks is a vector that keeps track of which landmarks have been observed so far.
% observedLandmarks(i) will be true if the landmark with id = i has been observed at some point by the robot
observedLandmarks = repmat(false,1,N);

% Initialize belief:
% mu: 2N+3x1 vector representing the mean of the normal distribution
% The first 3 components of mu correspond to the pose of the robot,
% and the landmark poses (xi, yi) are stacked in ascending id order.
% sigma: (2N+3)x(2N+3) covariance matrix of the normal distribution
mu = repmat([0.0], (2*N+3), 1);
robSigma = zeros(3);
robMapSigma = zeros(3,2*N);
mapSigma = INF*eye(2*N);
sigma = [[robSigma robMapSigma];[robMapSigma' mapSigma]];

% toogle the visualization type
%showGui = true;  % show a window while the algorithm runs
showGui = false; % plot to files instead

% Perform filter update for each odometry-observation pair read from the
% data file.
for t = 1:size(data.timestep, 2)
%for t = 1:80

    % Perform the prediction step of the EKF
    [mu, sigma] = prediction_step(mu, sigma, data.timestep(t).odometry);

    % Perform the correction step of the EKF
    [mu, sigma, observedLandmarks] = correction_step(mu, sigma, data.timestep(t).sensor, observedLandmarks);

    %Generate visualization plots of the current state of the filter
    plot_state(mu, sigma, landmarks, t, observedLandmarks, data.timestep(t).sensor, showGui);
    disp("Current state vector:")
    disp("mu = "), disp(mu)
endfor

disp("Final system covariance matrix:"), disp(sigma)
% Display the final state estimate
disp("Final robot pose:")
disp("mu_robot = "), disp(mu(1:3)), disp("sigma_robot = "), disp(sigma(1:3,1:3))


function [ind,ind2,stats] = Gaussian_mix(x,niter,basepts,verbose,doplot, doplot2)
% Two-Gaussian mixture model
%
% :Usage:
% ::
%
%     [ind,ind2,stats] = Gaussian_mix(x,niter,basepts,verbose,doplot, doplot2)
%
% :Inputs:
%
%   **x:**
%        data
%
%   **iter:**
%        number of iterations
%
%   **basepts:**
%        number of baseline pts at start of run. The modal class in the
%        baseline period is defined as the 0-class
%
% :Outputs:
%
%   **ind:**
%        indicator function of class belonging
%
%   **ind2:**
%        indicator function of class belonging where 3 consecutive points
%        are needed to switch states
%
%   **mu:**
%        mean vector
%
%   **sigma:**
%        standard deviation
%
%   **p:**
%        probability that latent class random variable (delta) is equal
%        to [0,1]
%
% :Examples:
% ::
%
%    [ind,ind2,stats] = Gaussian_mix(linear_detrending(dat),20);
%    % dat is n subjects by t time points
%    % plotting is on
%
%    % Simulated data
%    r = normrnd(1, 1, 200, 1); r(1:50) = r(1:50) + normrnd(3, 1, 50, 1);
%    [ind, ind2, stats] = Gaussian_mix(r, 50, [], 0, 1);
%
% We recommend 50 iterations

% ..
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Set up inputs
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ..

    if ~(exist('doplot')==1) || isempty(doplot), doplot = 1; end
    if ~(exist('doplot2')==1) || isempty(doplot2), doplot2 = 1; end
    if ~(exist('verbose')==1) || isempty(verbose), verbose = 1; end
    if ~(exist('basepts')==1) || isempty(basepts), basepts = 1:length(x); end

    % make sure it's a column vector or vectors
    if size(x,1) ~= length(x), x = x'; end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Iterative mode
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if size(x,2) > 1
        % multiple data vectors, run this function iteratively

        IND = []; IND2 = [];
        verbose = 0;
        if doplot, nrows = ceil(sqrt(size(x,2))); tor_fig(nrows,nrows); end

        for datavec = 1:size(x,2)
            if doplot, subplot(nrows,nrows,datavec);  end

            [IND(:,datavec),IND2(:,datavec),stats(datavec)] = Gaussian_mix(x(:,datavec),niter,basepts,verbose,doplot);
        end

        % save group stats/output

        ind = IND; ind2 = IND2;
        S.cp = cat(1,stats.cp);
        S.cnt = cat(1,stats.cnt);
        S.tot = cat(1,stats.tot);
        S.longest = cat(1,stats.longest);
        S.firstlen = cat(1,stats.firstlen);
        S.totaldur = cat(1,stats.totaldur);
        S.mu = cat(2,stats.mu);
        S.sigma = cat(2,stats.sigma);
        S.p = cat(1,stats.p);
        S.ind2 = IND2;
        S.ind = ind;

        S.cpmean = nanmean(S.cp);
        S.meancnt = mean(S.cnt);
        S.meantot = mean(S.tot);
        S.meantotaldur =  mean(S.totaldur);
        S.meanlongest = mean(S.longest);
        S.meanfirst = mean(S.firstlen);

        ind = mean(IND2')';     % group prob. of activation state
        stats = S;

        if doplot
            % group plot
            tor_fig; plot(ind,'k','LineWidth',2);
            hold on; plot(S.cpmean,ind(round(S.cpmean)),'ko','MarkerSize',12,'MarkerFaceColor',[.5 .5 .5]);
            xlabel('Time (images)'); ylabel('Probability of active state.');
        end

        return
    end

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

    %

    % Initial values for EM-algorithm

    %

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

    n = length(x);

    gam =zeros(n,2);

    p = [0.5 0.5];                      % probability that delta is [0,1]

    %mu = normrnd(0,1,1,2);              % means of Gaussian

    mu = zeros(1,2);
    mu(1) = min(x);
    mu(2) =max(x);

    sigma = zeros(1,1,2);               % covariance matrices

    for i=1:2,

        sigma(:,:,i) = std(x);

    end

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

    %

    % EM -algorithm - repeat niter times

    %

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

    if verbose,  fprintf(1,'iteration %03d',0); end

    for t=1:niter,

        if verbose, fprintf(1,'\b\b\b%03d',t); end

        % E-step

        for i=1:2,

            gam(:,i) = p(i)*det(sigma(:,:,i))^(-0.5)*exp(-0.5*sum((x'-repmat(mu(:,i),1,n))'*inv(sigma(:,:,i)).*(x'-repmat(mu(:,i),1,n))',2));

        end

        gam = gam./repmat(sum(gam,2),1,2);            % Normalize

        % M-step

        for i=1:2,

            mu(:,i) = (x'*gam(:,i))./sum(gam(:,i));                                                               % Update mean

            ind = (gam>0.5);
            dev = x-repmat(mu(:,i),n,1);

            sigma(:,:,i) = dev' *(gam(:,i).* dev) ./ sum(gam(:,i));           % Update covariance

            p(i) = mean(gam(:,i));                                                                                % Update probability

        end

        pooleds = sigma(:,:,1).*p(1) + sigma(:,:,2).*p(2);
        sigma(:,:,1:2) = pooleds;

    end

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

    %

    % Classify points

    %

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

    ind = (gam>0.5);

    [a,b]=max(mu);

    % Classify points using 3 consecutive alternate states in order to

    % switch states

    ind2 = zeros(n,1);

    state = 0;

    len = 3;

    for i=1:(length(ind(:,b))-(len-1)),

        if (state == 0),

            if(sum(ind(i:(i+(len-1)),b)) == len),

                state = 1;

            end;

        elseif (state == 1),

            if(sum(ind(i:(i+(len-1)),b)) == 0),

                state = 0;

            end;

        end;

        ind2(i) = state;

    end;

    ind2((length(ind(:,b))-(len-1)):end) = state;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % re-format output and do baseline pts.
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    ind = ind(:,1) - ind(:,2);
    %ind2 = ind2(:,1) - ind2(:,2);

    % wh is the class number (0 = class #1, 1 = class #2) of the most frequent
    % baseline class
    classes = [0 1];
    wh = [sum(ind2(1:basepts) == 0) sum(ind2(1:basepts) == 1)]; wh = find(wh==max(wh)); wh = wh(1);
    baseclass = classes(wh);    % 0 or 1

    whbase = find(ind2 == baseclass);
    whactive = find(ind2 ~= baseclass);

    % define so that base class is 0, active class is 1
    ind2(whbase) = 0; ind2(whactive) = 1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % output stats and stats on runs
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    stats.cp = find(ind2);  % first point in active state -- CP estimate
    if isempty(stats.cp), stats.cp = NaN;
    else
        stats.cp = stats.cp(1);
    end

    [stats.cnt,stats.tot,lenmat] = cnt_runs(ind2);
    stats.longest = max(lenmat); stats.firstlen = lenmat(1);
    stats.totaldur = find(ind2);
    if isempty(stats.totaldur) || length(stats.totaldur) < 2, stats.totaldur = 0;
    else
        stats.totaldur = stats.totaldur(end) - stats.totaldur(1);
    end
    stats.mu = mu'; stats.sigma = squeeze(sigma); stats.p = p; stats.gam = gam;

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

    %

    % Plot results

    %

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

    if doplot

        xx = 1:length(x);

        hold off;
        plot(xx,x,'k','LineWidth',1);
        hold on;

        wh = find(~ind2); xtmp = x; xtmp(wh) = NaN;
        plot(xx,xtmp,'b','LineWidth',2);

        wh = find(ind2);xtmp = x; xtmp(wh) = NaN;
        plot(xx,xtmp,'g','LineWidth',2);

        %plot(ind2,'r','LineWidth',2);

        %axis([0 length(ind) -0.1 1.1])

    end

    if doplot2

        [h, x] = hist(x, ceil(length(x) ./ 5));
        h1 = normpdf(x, stats.mu(1), stats.sigma(1));
        h2 = normpdf(x, stats.mu(2), stats.sigma(2));

        stats.cnt1 = sum(ind > 0);
        stats.cnt2 = sum(ind < 0);

        try
            h1 = moving_average('gaussian', h1', 8)';
            h2 = moving_average('gaussian', h2', 8)';
        catch
            disp('problem with moving average')
        end

        h1 = stats.cnt1 .* h1 ./ sum(h1);
        h2 = stats.cnt2 .* h2 ./ sum(h2);

        create_figure('Gaussian mixture plot');
        plot(x, h, 'k', 'LineWidth', 2);
        hold on;
        plot(x, h1, 'r', 'LineWidth', 2);
        plot(x, h2, 'b', 'LineWidth', 2);

    end

    return


% Extract critical features (junctions and endpoints).
%
% Rougly follows procedure of Liu et al. "Identification of Fork Points..." IEEE
% TAPMI
%
% For a circular stroke, there are no "features" by this definition.
% Thus, for any partition of the pixels into regions, there should
% be at least one feature for the tracing algorithm to find.
%
% Input
%  T: [n x n boolean] thinned image.
%    images are binary, where true means "black"
%
% Output
%  SN: [n x n boolean] extracted features.
function SN = extract_junctions(T)

    SE = bwmorph(T,'endpoints');
    SB = T; % black pixels
    sz = size(T,1);

    lutS3 = makelut( @(P)fS3(P) , 3);
    S3 = applylut(T,lutS3);

    % final criteria
    SN = SE | (SB & S3);

    % Check to see that each connected component has a feature.
    % This is necessary to process circles in the image.
    CC = bwconncomp(T,8);
    nCC = CC.NumObjects;
    for c=1:nCC

       pid = CC.PixelIdxList{c};

       % We have a circle. Circles are generally drawn from the
       % top, we choose the top pixel here
       if sum(SN(pid))==0
          [irow,icol] = ind2sub(sz,pid);
          sel = argmin(irow);
          SN(pid(sel)) = true;
       end

    end

end

% See Liu et al.
function Y=fS3(P)
    sz = size(P);
    assert(isequal(sz,[3 3]));

    % Get cross number
    NC = fNC(P);

    % Count black pixels
    PM = P;
    PM(2,2) = false;
    NB = sum(PM(:));

    % Criteria
    Y = (NC >= 3-eps) || (NB >= 4-eps);
end

% See Liu et al.
function Y=fNC(P)
    sum = 0;
    for i=0:7
        sum = sum + abs( P(fIP(i+1)) - P(fIP(i)) );
    end
    Y = sum./2;
end

% See Liu et al.
function newlindx = fIP(lindx)
    switch lindx
        case {0,8}
            i=1; j=2;
        case 1
            i=1; j=3;
        case 2
            i=2; j=3;
        case 3
            i=3; j=3;
        case 4
            i=3; j=2;
        case 5
            i=3; j=1;
        case 6
            i=2; j=1;
        case 7
            i=1; j=1;
    end
    newlindx = sub2ind([3 3],i,j);
end


% Weighted Prediction Error for dereverberation
% reference: Nakatani, Tomohiro, et al. "Speech dereverberation based on
% variance-normalized delayed linear prediction." IEEE TASLP 18.7 (2010)
% ZitengWANG@201903

clear all
addpath('..\STFT\')
addpath('..\Simulation\')
addpath('..\Simulation\RIR-Generator\')

%% simulation start
flatStart = 1;
prefix = '';   % for saving file

speechDir = '..\Simulation\Data\';
speechFile = 'fajw0_sa1.wav';
saveDir = 'GeneratedData\';
if ~exist(saveDir)
    mkdir(saveDir)
end
speech = audioread([speechDir speechFile]);

% configuration
cfg = [];
cfg.fs = 16000;                     % sampling rate
cfg.room = [6 5 3];                 % room dimension (m)
cfg.T60 = 0;                      % reverberation time (s)

cfg.Nch = 6;
cfg.micCenter = [2 3 1.5];          % array center (m)
cfg.micCoordinate =[0.0425,0.0,0.0;
        0.02125,0.03680608,0.0;
        -0.02125,0.03680608,0.0;
        -0.0425,0.0,0.0;
        -0.02125,-0.03680608,0.0;
        0.02125,-0.03680608,0.0;];  % microphone array coordinates

cfg.az = 180;
cfg.el = 0;
cfg.dist = 3;

cfg.SNR = 20;
if cfg.SNR ~= inf               % inf means no noise
    cfg.noiseType = 'white';    % choice {'white' 'diffuse' 'recorded'}
    if strcmp(cfg.noiseType, 'recorded')
        % check first the noise is longer than speech!
        cfg.noiseFile = '';
    end
end

cfg.SIR = inf;

% setup room and collect data
setup_room
setup_noise

%% offline processing start
% the following parts are specific to the algorithm
Nfft = 512;

Y = stft_multi_2(y, Nfft);
[Nframe, Nbin, Nch] = size(Y);

iterMax = 5;
wpe.delay = 3;
wpe.taps = 10;
XeEst = Y;
for bin = 1:Nbin
    % get audio context first
    YbarFrm = cell(Nframe, 1);
    for frm = (wpe.taps + wpe.delay):Nframe
        Ybar = squeeze(Y(frm - wpe.delay,bin,:));
        for tap=1:wpe.taps-1
            Ybar = [Ybar; squeeze(Y(frm - wpe.delay - tap,bin ,:))];
        end
        YbarFrm{frm} = Ybar;
    end

    % iterate
    X = squeeze(Y(:,bin,:));
    for iter = 1:iterMax
        % calculate mean signal power
        Xpow = mean(abs(X).^2, 2);
        XpowMax = max(Xpow);
        % calculate correlation matrix and correlation vector
        R = 0;
        P = 0;
        for frm = (wpe.taps + wpe.delay):Nframe
            Ybar = YbarFrm{frm};
            YbarTmp = Ybar / max(Xpow(frm), 1e-10*XpowMax);
            R = R + YbarTmp * Ybar' ;
            P = P + YbarTmp * squeeze(Y(frm,bin,:))';
        end
        hWPE = R \ P;

        % apply the filter
        for frm = (wpe.taps + wpe.delay):Nframe
            X(frm, :) = squeeze(Y(frm,bin,:)) - hWPE'*YbarFrm{frm};
        end
    end
    XeEst(:,bin,:) = permute(X,[1,3,2]);
end

xeEst = istft_multi_2(XeEst, length(speech));
audiowrite([saveDir prefix 'WPE' postfix '.wav'], xeEst, fs);


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

function [S, u, d, df, p] = createbintree(S0, T, n, r, sigma)
% creates a simple binomial tree using repmat
    dt = T / n;                  % length of one period
    u = exp(sigma * sqrt(dt));   % up move
    d = 1 / u;                   % down move
    df = exp(-r * dt);           % discount
    p = (1/df - d) / (u - d);    % probability up

    S = zeros(2^n, n+1);
    S(:,1) = S0*ones(2^n,1);

    for i=1:1:n
        a = [u * ones(2^(n-i),1); d * ones(2^(n-i),1)];
        S(:,i+1) = S(:,i).*repmat(a, 2^(i-1),1);
    end

end


function s = resampdet(p,m,n);
%RESAMPDET Deterministic resampling
%
%   Description
%   S = RESAMPDET(P) returns a new set of indices according to the
%   probabilities P. P is array of probabilities, which are not
%   necessarily normalized, though they must be non-negative, and
%   not all zero. The size of S is the size of P.
%
%   S = RESAMPDET(P,M,N) returns M by N matrix.
%
%   S = RESAMPDET(P,M) returns M by M matrix.
%
%   Default is to use no-sort resampling. For sorted resampling use
%    [PS,PI]=SORT(P);
%    S=PI(RESAMPDET(PS));
%   Sorted re-sampling is slower but has smaller variance. Note
%   that deterministic resampling is not unbiased. Stratified
%   resampling (RESAMPSTR) is unbiased, almost as fast as
%   deterministic resampling, and has only slightly larger
%   variance.
%
%   In deterministic resampling indices are sampled using
%   deterministic numbers u_j~(j-a)/n, for fixed a in [0,1) and
%   n is length of P. Compare this to simple random resampling
%   where u_j~U[0,n]. See, Kitagawa, G., Monte Carlo Filter and
%   Smoother for Non-Gaussian Nonlinear State Space Models,
%   Journal of Computational and Graphical Statistics, 5(1):1-25,
%   1996.
%
%   See also RESAMPSIM, RESAMPRES, RESAMPSTR

% Copyright (c) 2003-2004 Aki Vehtari

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

if nargin<2
    [m,n]=size(p);
elseif nargin==2
    n=m;
end
mn=m.*n;
pn=p./sum(p(:)).*mn;
fpn=floor(pn);
s=zeros(m,n);
k=0;
c=0.5;
for i=1:numel(p)
  if pn(i)>=1
    a=fpn(i);
    pn(i)=pn(i)-a;
    s(k+[1:a])=i;
    k=k+a;
  end
  c=c+pn(i);
  if c>=1
    k=k+1;
    s(k)=i;
    c=c-1;
  end
end


function angles3d(varargin)
%ANGLES3D Conventions for manipulating angles in 3D.
%
%   The library uses both radians and degrees angles;
%   Results of angle computation between shapes usually returns angles in
%   radians.
%   Representation of 3D shapes use angles in degrees (easier to manipulate
%   and to save).
%
%   Contrary to the plane, there are no oriented angles in 3D. Angles
%   between lines or between planes are comprised between 0 and PI.
%
%   Spherical angles
%   Spherical angles are defined by 2 angles:
%   * THETA, the colatitude, representing angle with Oz axis (between 0 and
%       PI)
%   * PHI, the azimut, representing angle with Ox axis of horizontal
%       projection of the direction (between 0 and 2*PI)
%
%   Spherical coordinates can be represented by THETA, PHI, and the
%   distance RHO to the origin.
%
%   Euler angles
%   Some functions for creating rotations use Euler angles. They follow the
%   ZYX convention in the global reference system, that is eqivalent to the
%   XYZ convention ine a local reference system.
%   Euler angles are given by a triplet of angles [PHI THETA PSI] that
%   represents the succession of 3 rotations:
%   * rotation around X by angle PSI    ("roll")
%   * rotation around Y by angle THETA  ("pitch")
%   * rotation around Z by angle PHI    ("yaw")
%
%   In this library, euler angles are given in degrees. The functions that
%   use euler angles use the keyword 'Euler' in their name.
%
%
%   See also
%   cart2sph2, sph2cart2, cart2sph2d, sph2cart2d
%   anglePoints3d, angleSort3d, sphericalAngle, randomAngle3d
%   dihedralAngle, polygon3dNormalAngle, eulerAnglesToRotation3d
%   rotation3dAxisAndAngle, rotation3dToEulerAngles
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2008-10-13,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.


function [G, Y, optinf] = bpdndl(D0, S, lambda, opt)

% bpdndl -- BPDN Dictionary Learning
%
%         argmin_{D,X} (1/2)||D X - S||_2^2 + lambda ||X||_1
%
%         Dictionary learning consists of interleaved alternation of
%         the ADMM (see boyd-2010-distributed) steps for the BPDN (see
%         chen-1998-atomic) and MOD (see engan-1999-method) problems.
%
% Usage:
%       [D, X, optinf] = bpdndl(D0, S, lambda, opt)
%
% Input:
%       D0          Initial dictionary
%       S           Input image
%       lambda      Regularization parameter
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       D           Dictionary
%       X           Coefficients
%       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, and
%                    primal and dual residuals (see Sec. 3.3 of
%                    boyd-2010-distributed). The values of rho and sigma
%                    are also displayed if options request that they are
%                    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)
%   L1Weight         Weight matrix for L1 norm
%   Y0               Initial value for Y
%   U0               Initial value for U
%   G0               Initial value for G (overrides D0 if specified)
%   H0               Initial value for H
%   rho              Augmented Lagrangian 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
%   sigma            Augmented Lagrangian penalty parameter
%   AutoSigma        Flag determining whether sigma is automatically
%                    updated (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoSigmaPeriod  Iteration period on which sigma is updated
%   SigmaRsdlRatio   Primal/dual residual ratio in sigma update test
%   SigmaScaling     Multiplier applied to sigma when updated
%   AutoSigmaScaling Flag determining whether SigmaScaling value is
%                    adaptively determined (see wohlberg-2015-adaptive). If
%                    enabled, SigmaScaling specifies a maximum allowed
%                    multiplier instead of a fixed multiplier.
%   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)
%   XRelaxParam      Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed) for X update
%   DRelaxParam      Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed) for D update
%   NonNegCoef       Flag indicating whether solution should be forced to
%                    be non-negative
%   AuxVarObj        Flag determining whether objective function is computed
%                    using the auxiliary (split) variable
%   ZeroMean         Force learned dictionary entries to be zero-mean
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-07-30
%
% 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 < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);
Nx = size(D0,2)*size(S,2);
Nd = prod(size(D0));

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

% Mean removal and normalisation projections
Pzmn = @(x) bsxfun(@minus, x, mean(x,1));
Pnrm = @(x) normalise(x);

% Projection of dictionary filters onto constraint set
if opt.ZeroMean,
  Pcn = @(x) Pnrm(Pzmn(x));
else
  Pcn = @(x) Pnrm(x);
end

% Start timer
tstart = tic;

% Project initial dictionary onto constraint set
D = Pnrm(D0);

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
sigma = opt.sigma;
if isempty(sigma), sigma = size(S,2)/200; end;
optinf = struct('itstat', [], 'opt', opt);
rx = Inf;
sx = Inf;
rd = Inf;
sd = Inf;
eprix = 0;
eduax = 0;
eprid = 0;
eduad = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(size(D,2), size(S,2));
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(size(D,2), size(S,2), class(S));
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end
if isempty(opt.G0),
  G = D;
else
  G = opt.G0;
end
Gprv = G;
if isempty(opt.H0),
  if isempty(opt.G0),
    H = zeros(size(G), class(S));
  else
    H = G;
  end
else
  H = opt.H0;
end
GS = G'*S;

% Main loop
k = 1;
while k <= opt.MaxMainIter && (rx > eprix|sx > eduax|rd > eprid|sd >eduad),

  % Solve X subproblem, using G as the dictionary for improved stability
  [luLx, luUx] = factorise(G, rho);
  X = linsolveX(G, rho, luLx, luUx, GS + rho*(Y - U));

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

  % Solve Y subproblem
  Y = shrink(Xr + U, (lambda/rho)*opt.L1Weight);
  if opt.NonNegCoef,
    Y(Y < 0) = 0;
  end
  SY = S*Y';

  % Update dual variable corresponding to X, Y
  U = U + Xr - Y;

  % Compute primal and dual residuals and stopping thresholds for X update
  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    rx = norm(vec(X - Y));
    sx = norm(vec(rho*(Yprv - Y)));
    eprix = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    eduax = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    rx = norm(vec(X - Y))/max(nX,nY);
    sx = norm(vec(Yprv - Y))/nU;
    eprix = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    eduax = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Solve D subproblem, using Y as the coefficients for improved stability
  [luLd, luUd] = factorise(Y, sigma);
  D = linsolveD(Y, sigma, luLd, luUd, SY + sigma*(G - H));

  % See pg. 21 of boyd-2010-distributed
  if opt.DRelaxParam == 1,
    Dr = D;
  else
    Dr = opt.DRelaxParam*D + (1-opt.DRelaxParam)*G;
  end

  % Solve G subproblem
  G = Pcn(Dr + H);
  GS = G'*S;

  % Update dual variable corresponding to D, G
  H = H + Dr - G;

  % Compute primal and dual residuals and stopping thresholds for D update
  nD = norm(D(:)); nG = norm(G(:)); nH = norm(H(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    rd = norm(vec(D - G));
    sd = norm(vec(sigma*(Gprv - G)));
    eprid = sqrt(Nd)*opt.AbsStopTol+max(nD,nG)*opt.RelStopTol;
    eduad = sqrt(Nd)*opt.AbsStopTol+sigma*nH*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    rd = norm(vec(D - G))/max(nD,nG);
    sd = norm(vec(Gprv - G))/nH;
    eprid = sqrt(Nd)*opt.AbsStopTol/max(nD,nG)+opt.RelStopTol;
    eduad = sqrt(Nd)*opt.AbsStopTol/(sigma*nH)+opt.RelStopTol;
  end

  % Objective function
  if opt.AuxVarObj,
    Jdf = sum(vec(abs(G*Y - S).^2))/2;
    Jl1 = sum(abs(vec(opt.L1Weight .* Y)));
  else
    Jdf = sum(vec(abs(D*X - S).^2))/2;
    Jl1 = sum(abs(vec(opt.L1Weight .* X)));
  end
  Jfn = Jdf + lambda*Jl1;
  Jcn = norm(vec(Pcn(D) - D));

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat; ...
        [k Jfn Jdf Jl1 rx sx rd sd eprix eduax eprid eduad rho sigma tk]];
  if opt.Verbose,
    dvc = [k Jfn Jdf Jl1 Jcn rx sx rd sd];
    if opt.AutoRho,
      dvc = [dvc rho];
    end
    if opt.AutoSigma,
      dvc = [dvc sigma];
    end
    disp(sprintf(sfms, dvc));
  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(rx/sx);
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if rx > opt.RhoRsdlRatio*sx, rsf = rhomlt; end
      if sx > opt.RhoRsdlRatio*rx, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
    end
  end
  if opt.AutoSigma,
    if k ~= 1 && mod(k, opt.AutoSigmaPeriod) == 0,
      if opt.AutoSigmaScaling,
        sigmlt = sqrt(rd/sd);
        if sigmlt < 1, sigmlt = 1/sigmlt; end
        if sigmlt > opt.SigmaScaling, sigmlt = opt.SigmaScaling; end
      else
        sigmlt = opt.SigmaScaling;
      end
      ssf = 1;
      if rd > opt.SigmaRsdlRatio*sd, ssf = sigmlt; end
      if sd > opt.SigmaRsdlRatio*rd, ssf = 1/sigmlt; end
      sigma = ssf*sigma;
      H = H/ssf;
    end
  end

  Yprv = Y;
  Gprv = G;
  k = k + 1;

end

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

if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

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

return

function u = normalise(v)

  vn = sqrt(sum(v.^2, 1));
  vn(vn == 0) = 1;
  u = bsxfun(@rdivide, v, vn);

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 = linsolveX(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 x = linsolveD(A, c, L, U, b)

  [N,M] = size(A);
  if N >= M,
    x = (b - (((b*A) / U) / L)*A')/c;
  else
    x = (b / U) / L;
  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 = 1e-6;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
    opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'G0'),
    opt.G0 = [];
  end
  if ~isfield(opt,'H0'),
    opt.H0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 0;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 10;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 2;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 0;
  end
  if ~isfield(opt,'sigma'),
    opt.sigma = [];
  end
  if ~isfield(opt,'AutoSigma'),
    opt.AutoSigma = 0;
  end
  if ~isfield(opt,'AutoSigmaPeriod'),
    opt.AutoSigmaPeriod = 10;
  end
  if ~isfield(opt,'SigmaRsdlRatio'),
    opt.SigmaRsdlRatio = 10;
  end
  if ~isfield(opt,'SigmaScaling'),
    opt.SigmaScaling = 2;
  end
  if ~isfield(opt,'AutoSigmaScaling'),
    opt.AutoSigmaScaling = 0;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'XRelaxParam'),
    opt.XRelaxParam = 1;
  end
  if ~isfield(opt,'DRelaxParam'),
    opt.DRelaxParam = 1;
  end
  if ~isfield(opt,'NonNegCoef'),
    opt.NonNegCoef = 0;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 1;
  end
  if ~isfield(opt,'ZeroMean'),
    opt.ZeroMean = 0;
  end

return


% Least Mean Squares algorithm
%
% From A. H. Sayed, "Fundamentals of adaptive filtering}", Wiley-IEEE
% Press, 2003, Chapter 5.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef lms < linear_filter

    properties (GetAccess = 'public', SetAccess = 'private')
        mu = 0.001; % learning rate
    end

    properties (GetAccess = 'public', SetAccess = 'private')
        w = []; % filter coefficients
    end

    methods
        function obj = lms(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(obj))
                        obj.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(obj,x) % evaluate the algorithm
            if numel(obj.w)>0
                y_est = x*obj.w;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(obj,x,y) % train the algorithm
            if numel(obj.w)==0 % initialize
                obj.w = zeros(length(x),1);
            end

            % Algorithm 5.2.1 in reference
            err = y - x*obj.w; % instantaneous error
            obj.w = obj.w + obj.mu*x'*err; % update filter coefficients
        end

    end
end


function A = slgda(K, nums, sol)
%SLGDA Performs Baudat's Generalized Discriminant Analysis
%
% $ Syntax $
%   - A = slgda(K, nums)
%   - A = slgda(K, nums, sol)
%
% $ Arguments $
%   - K:        the kernel gram matrix
%   - nums:     the numbers of samples in each classes
%   - sol:      the cell containing the parameter for generalized eigen
%               decomposition
%   - A:        the resulting projection coefficient matrix
%
% $ Description $
%   - A = slgda(K, nums) performs Generalized Discriminant Analysis(GDA),
%     an representative work in using kernel method to extend LDA,
%     proposed by Baudat et al. The generalized eigen-problem is
%     solved in a default way by slsymgeig.
%
%   - A = slgda(K, nums, sol) in the function, slsymgeig will be invoked
%     to solve the generalized eigen-decomposition problem. sol is
%     a cell containing the parameters for slsymgeig.
%
% $ Remarks $
%   - The function follows the instructions given in the original paper
%     on GDA.
%
%   - The projection is learned after the kernel gram matrix is
%     centralized.
%
%   - The aim of the function is to give an exact implementation of
%     of representative work GDA, so it does not offer other facilities
%     such as weighting and other ways of scatter computation. For higher
%     flexibility, please use the function slkfd.
%
% $ History $
%   - Created by Dahua Lin on May 3rd, 2005
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slkernelscatter', 2);
end

if ndims(K) ~= 2 || size(K, 1) ~= size(K, 2)
    error('sltoolbox:invaliddims', ...
        'The gram matrix K should be a square matrix');
end

M = size(K, 1);         % number of samples
N = length(nums);       % number of classes
if ~isequal(size(nums), [1, N])
    error('sltoolbox:invaliddims', ...
        'The nums should be a 1 x N row vector');
end
if sum(nums) ~= M
    error('sltoolbox:sizmismatch', ...
        'The total number in nums is inconsistent with that in K');
end

if nargin < 3
    sol = {};
end

%% Compute

%% Centralize

K = slcenkernel(K);

%% Construct the eigen-problem

[sp, ep] = slnums2bounds(nums);
W = zeros(M, M);
for i = 1 : N
    ni = nums(i);
    spi = sp(i); epi = ep(i);
    W(spi:epi, spi:epi) = 1 / ni;
end
clear sp ep;

B = K * W * K;
clear W;

V = K * K;

%% Resolve the eigen-problem

[evs, A] = slsymgeig(B, V, sol{:});
d = sldim_by_eigval(evs);
A = A(:, 1:d);


classdef CEC2008_F5 < PROBLEM
% <single> <real> <large/none> <expensive/none>
% Shifted Griewank's function

%------------------------------- Reference --------------------------------
% K. Tang, X. Yao, P. N. Suganthan, C. MacNish, Y.-P. Chen, C.-M. Chen, and
% Z. Yang, Benchmark functions for the CEC'2008 special session and
% competition on large scale global optimization, Nature Inspired
% Computation and Applications Laboratory, USTC, China, 2007.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2008.mat'),'Data');
            obj.O = Data{5};
            obj.M = 1;
            if isempty(obj.D); obj.D = 100; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 600;
            obj.upper    = zeros(1,obj.D) + 600;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = 1/4000*sum(Z.^2,2) - prod(cos(Z./sqrt(repmat(1:size(Z,2),size(Z,1),1))),2) + 1;
        end
    end
end


function y = modulate2(x, type, center)
% MODULATE2	2D modulation
%
%	y = modulate2(x, type, [center])
%
% With TYPE = {'r', 'c' or 'b'} for modulate along the row, or column or
% both directions.
%
% CENTER secify the origin of modulation as floor(size(x)/2)+1+center
% (default is [0, 0])

if ~exist('center', 'var')
    center = [0, 0];
end

% Size and origin
s = size(x);
o = floor(s / 2) + 1 + center;

n1 = [1:s(1)] - o(1);
n2 = [1:s(2)] - o(2);

switch lower(type(1))
    case 'r'
	m1 = (-1) .^ n1;
	y = x .* repmat(m1', [1, s(2)]);

    case 'c'
	m2 = (-1) .^ n2;
	y = x .* repmat(m2, [s(1), 1]);

    case 'b'
	m1 = (-1) .^ n1;
	m2 = (-1) .^ n2;
	m = m1' * m2;
	y = x .* m;

    otherwise
	error('Invalid input type');
end


function uknt = kntunclamp (knt, deg, k, dim)
% KNTUNCLAMP: Compute the unclamped knot vector starting from an open one.
%
% Calling Sequence:
%
%   uknt = kntunclamp (knt, deg, k)
%   uknt = kntunclamp (knt, deg, k, dim)
%
% INPUT:
%
%   knt	: open knot vector: see kntrefine
%   deg : polynomial degree of the spline space
%   k   : continuity for the unclamping (from 0 up to p-1)
%   dim : dimensions in which to unclamp (all by default).
%
% OUTPUT:
%
%   uknt: unclamped knot vector, see nrbmak
%
% Description:
%
%     Unclamps directly the open knot vector. See nrbunclamp
%    for further information.
%
%    Copyright (C) 2013, 2014 Rafael Vazquez
%    Copyright (C) 2020, Bernard Kapidani
%
%    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/>.

  knt_is_cell = true;
  if (~iscell (knt))
    knt = {knt};
    knt_is_cell = false;
  end
  uknt = knt;

  ndim = numel (knt);
  if (nargin < 4)
    dim = 1:ndim;
  end

% if (iscell (knt))
  if (numel(k) < ndim)
    k = [k(:).', k(end) * ones(1, ndim-numel(k))];
  end

  assert (numel(deg) == ndim, 'degrees and knots must have the same size');
  for idim = dim

    U  = knt{idim};

    p  = deg(idim);
    n  = numel(U) - p - 1;
    m  = n + p + 1;
    kk = k(idim);

    if (kk >= p)
      warning ('Taking the maximum k allowed, degree - 1')
      kk = p - 1;
    end

  % Unclamp at left end
    for ii=0:kk
      U(kk-ii+1) = U(kk-ii+2) - (U(n+1-ii) - U(n-ii));
    end

  % Unclamp at right end
    for ii=0:kk
      U(m-kk+ii) = U(m-kk+ii-1) + U(p+ii+1+1) - U(p+ii+1);
    end

    uknt{idim} = U;

  end

  if (~knt_is_cell)
    uknt = uknt{1};
  end

end