%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 D = spm_mrdivide(A, B)
% Regularised variant of mrdivide(A, B) or A / B, similar to B * spm_inv(A)
% FORMAT D = spm_mrdivide(A, B)
%
% D = B * inv(A), or if A is near singular D = B * inv(A + TOL*eye(size(A))
%
% where TOL is adaptively increased if necessary.
%
% This function should be preferable to B * spm_inv(A) if A is large and
% sparse or if B has few rows, since the inverse need not be explicitly
% computed (the linear system can be solved with the backslash operator).
%
% See also: spm_mldivide
%__________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Ged Ridgway
% $Id: spm_mrdivide.m 4360 2011-06-14 16:46:37Z ged $

D = spm_mldivide(B', A')';


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 f = harmean(g,m,n)
% Implemets a harmonic mean filter.
inclass = class(g);
g = im2double(g);
f = m * n ./ imfilter(1./(g + eps),ones(m,n),'replicate');
f = changeclass(inclass,f);


function y=H_obj(x)

y=[8-2*x(2) -2*x(1);-2*x(1) 2];


function [Simil2]=fuzzysimil2(A,B)

PA=sqrt((A(1)-A(2))^2+A(5)^2)+sqrt((A(3)-A(4))^2+A(5)^2)+(A(3)-A(2))+(A(4)-A(1));
PB=sqrt((B(1)-B(2))^2+B(5)^2)+sqrt((B(3)-B(4))^2+B(5)^2)+(B(3)-B(2))+(B(4)-B(1));
aA=1/2*(A(5)*(A(3)-A(2)+A(4)-A(1)));
aB=1/2*(B(5)*(B(3)-B(2)+B(4)-B(1)));
temp=(min(PA,PB)/max(PA,PB))*(min(aA,aB) + min(A(5),B(5)))/(max(aA,aB) + max(A(5),B(5)));

Simil2=(1-(sum(abs(A(1:4)-B(1:4))))/4)*temp;


% demonstrate usage of classification
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch 2013-10-16.
clear all, close all

%% SAY WHICH CODE WE WISH TO EXERCISE
id = [1,1]; % use Gauss/Exact
id = [1,2; 2,2; 3,2]; % compare Laplace
id = [1,3; 2,3; 3,3]; % study EP
id = [1,5; 2,5]; % look into KL (takes quite a while)
id = [1,4; 2,4; 3,4]; % deal with VB

seed = 943; randn('seed',seed), rand('seed',seed)

ntr = 50; nte = 1e4;                        % number of training and test points
xtr = 10*sort(rand(ntr,1));                                     % sample dataset
p = @(x) 1./(1+exp(-5*sin(x)));                  % "true" underlying probability
ytr = 2*(p(xtr)>rand(ntr,1))-1;                              % draw labels +1/-1
i = randperm(ntr); nout = 3;                                      % add outliers
ytr(i(1:nout)) = -ytr(i(1:nout));
xte = linspace(0,10,1e4)';                    % support, we test our function on
cov = {@covSEiso}; sf = 1; ell = 0.7;                             % setup the GP
hyp0.cov  = log([ell;sf]);
mean = {@meanZero};                                                   % m(x) = 0
hyp0.mean = [];
lik_list = {'likGauss','likErf','likLogistic'};          % allowable likelihoods
inf_list = {'infExact','infLaplace','infEP','infVB','infKL'};   % inference algs

Ncg = 50;                                   % number of conjugate gradient steps
sdscale = 0.5;                  % how many sd wide should the error bars become?
col = {'k',[.8,0,0],[0,.5,0],'b',[0,.75,.75],[.7,0,.5]};                % colors
ymu{1} = 2*p(xte)-1; ys2{1} = 0;
for i=1:size(id,1)
  lik = lik_list(id(i,1));                                % setup the likelihood
  if strcmp(lik,'likGauss')
    sn = .2; hyp0.lik = log(sn);
  else
    hyp0.lik = [];
  end
  inf = inf_list{id(i,2)};
  fprintf('OPT: %s/%s\n',lik_list{id(i,1)},inf_list{id(i,2)})
  hyp = minimize(hyp0,'gp', -Ncg, inf, mean, cov, lik, xtr, ytr);   % opt hypers
  [ymu{i+1}, ys2{i+1}] = gp(hyp, inf, mean, cov, lik, xtr, ytr, xte);  % predict
  [nlZ(i+1)] = gp(hyp, inf, mean, cov, lik, xtr, ytr);
end

figure, hold on
for i=1:size(id,1)+1
  plot(xte,ymu{i},'Color',col{i},'LineWidth',2)
  if i==1
    leg = {'function'};
  else
    leg{end+1} = sprintf('%s/%s -lZ=%1.2f',...
                                lik_list{id(i-1,1)},inf_list{id(i-1,2)},nlZ(i));
  end
end
for i=1:size(id,1)+1
  ysd = sdscale*sqrt(ys2{i});
  fill([xte;flipud(xte)],[ymu{i}+ysd;flipud(ymu{i}-ysd)],...
       col{i},'EdgeColor',col{i},'FaceAlpha',0.1,'EdgeAlpha',0.3);
end
for i=1:size(id,1)+1, plot(xte,ymu{i},'Color',col{i},'LineWidth',2), end
plot(xtr,ytr,'k+'), plot(xtr,ytr,'ko'), legend(leg)


function Show_EWT2D_Filters(fil)

%===========================================================
%
% function Show_EWT2D_Filters(fil)
%
% This function permits to plot the Fourier magnitude of all
% 2D EWT Littlewood-Paley filters.
%
% Input:
%   - fil: cell containing all the filters
%
% Author: J.Gilles
% Institution: UCLA - Department of Mathematics
% email: jegilles@math.ucla.edu
% Date: March, 1st, 2013
%
%===========================================================
p=ceil(length(fil)/2);

figure;
for n=1:length(fil)
    subplot(2,p,n);imshow(fftshift(abs(fil{n})),[]);
end


function Jnst = newtonbc(J,xy,bound)
%newtonbc imposes Dirichlet bc on Jacobian
%   Jnst = newtonbc(J,xy,bound);
%   input
%          J          Jacobian velocity matrix
%          xy         vertex coordinate vector
%          bound      boundary vertex vector
%   output
%          Jnst       Jacobian velocity matrix
%
%   IFISS function: DJS, HCE; 30 August 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

nvtx = length(xy(:,1));
nu = nvtx*2; nbd=length(bound);
null_col=sparse(nu,nbd); %null_row=sparse(nbd,nu);
Jnst=J;

%% set boundary condition
xbd=xy(bound,1); ybd=xy(bound,2);
%% impose boundary condition
dA=zeros(nvtx,1); dA(bound)=ones(nbd,1);

%Procedure is equivalent to the two commented lines below
%It is more efficient because only columns are referenced
%Jnst(:,bound)=null_col;  Jnst(bound,:)=null_row;
%Jnst(:,nvtx+bound)=null_col;  Jnst(nvtx+bound,:)=null_row;

Jt = Jnst';
Jt(:,bound) = null_col;
Jt(:,nvtx+bound) = null_col;
Jnst = Jt';
Jnst(:,bound) = null_col;
Jnst(:,nvtx+bound) = null_col;

Jnst = Jnst + [spdiags(dA,0,nvtx,nvtx),sparse(nvtx,nvtx); ...
               sparse(nvtx,nvtx),spdiags(dA,0,nvtx,nvtx)];


function bin_pos = createIndexVector(keypts, match_binsize, x_bin_num, y_bin_num)
%CREATEINDEXVECTOR Find the bin position to which each keypoint belongs to
%
% INPUT:
%   - keypts(1, N): input feature keypoints
%   - match_binsize: matching bin width/height
%   - x_bin_num: number of bins along x-direction
%   - y_bin_num: number of bins along y-direction
%
% OUTPUT:
%   - bin_pos(x_bin_num, y_bin_num, 4): cell containing indices of keypoint

% allocate memory
num = size(keypts, 2);
% number of classes
classes_num = 4;

% create a array for bin positions/class
bin_pos = cell(x_bin_num, y_bin_num, classes_num);

% iterate over all keypoints
for i = 1:num
    % coordinate of keypoint
    x = keypts(i).location(1);
    y = keypts(i).location(2);
    c = keypts(i).class;
    % find bin position
    bin_x = min(ceil(x/match_binsize), x_bin_num);
    bin_y = min(ceil(y/match_binsize), y_bin_num);
    % add keypoint index to corresponding bin_pos
    bin_pos{bin_x, bin_y, c} = horzcat(bin_pos{bin_x, bin_y, c}, i);
end

end


function res = penalized_coefficients(lasso_object, indices, alignment)
% -------------------------------------------------------------------------
% function res = penalized_coefficients(penalized_object, indices, alignment)
% -------------------------------------------------------------------------
% PURPOSE: This function returns the LASSO coefficients for which the
%          chosen index equals those listed in indices.
%          The alignment index can be chosen to be:
%          lambdas, normalized L1 norms and L1 norms;
% -------------------------------------------------------------------------
% INPUTS:
% lasso_object: a structure as the one returned by the lasso routine:
% indices:      index of the points at which interpolation is wanted
% alignment:    one string of ('normalized_l1', 'l1'm 'lambdas')
%               indicating which index to use for alignment
% -------------------------------------------------------------------------
% OUTPUTS:
% res:          a structure containing  the coefficients and the
%               regularization parameter at the interpolated points
%               intercept:
%               nbeta:
%               beta:
%               lambda:
% -------------------------------------------------------------------------
% Author: Guilherme V. Rocha
%         Department of Statistics
%         University of California, Berkeley
%         gvrocha@stat.berkeley.edu, gvrocha@gmail.com
% 2006/09
% -------------------------------------------------------------------------
% See also: LASSO, BLASSOL2

% 0. Checking input parameters:
%==========================================================================
if nargin < 3
  alignment = 'normalized_penalty';
end;

% 1. Interpolates according to the chosen L1 norm (normalized x non normalized)
%==========================================================================
switch(lower(alignment))
  case{'normalized_penalty'}
    sizes = lasso_object.npenalty;
  case{'penalty'}
    sizes = lasso_object.penalty;
  case{'lambda'}
    sizes = lasso_object.lambda;
  otherwise
    error('Unrecognized alignment string');
end;

% Takes care of the case when there are entries with repeated L1s:
[unique_sizes, I, J]   = unique(sizes);

% If one of the points to be calculated is beyond the end of the path, use
% the end of path coeffcients for it:
indices       = min(indices, max(sizes));
indices       = reshape(indices, prod(size(indices)), 1);
res.intercept = interp1(unique_sizes, lasso_object.intercept(I), indices);
res.beta      = interp1(unique_sizes, lasso_object.beta(I,:), indices);
res.nbeta     = interp1(unique_sizes, lasso_object.nbeta(I,:), indices);
if(~isempty(lasso_object.lambda))
  res.lambda    = interp1(unique_sizes, lasso_object.lambda(I,:), indices);
end;
res.indices   = indices';


Fs=44100; % sampling frequency
dt=1/Fs;
T0=2; % period, sec

close all;

r = audiorecorder(Fs, 16, 1);
clc;disp('recording started...');
recordblocking(r,T0); % record next data
disp('recorded');
s00 = getaudiodata(r); % get data

L0=length(s00);

s=s00-mean(s00);

 %load('bp.mat'); % bandpass 50-4000 Hz
 load('lp.mat'); % lowpass 0-10000Hz

 s = filter(Num,1,s);

 s=s/max(abs(s)); % normalize to maximum

[op ismax]=find_all_optimums(s);
% op - optimums positions

%[z isrise]=find_zeros(s);
%op=decimate_optimums(z,op,s(op));

t=(0:L0-1)'*dt;
plot(t,s,'b-');
hold on;
plot(t(op),s(op),'rx');

% sw0=interp1(t(op),s(op),t,'nearest');
% sw1=interp1(t(op),s(op),t,'linear');
% al=0;

dop=diff(op);

% quntitize dop to [0 255]:
dop(dop>255)=255;
dop=round(dop); % not necessary

opv=s(op); % values in optimums

% quntitize opv to -128 127
opv=128*opv;
opv(opv>127)=127;
opv(opv<-128)=-128;
opv=round(opv);

% compression ratio:
szo=2*length(s); % original size as 2 byts per sample
szc=1*length(dop)+1*length(opv); % compressed size one byte for dop element, one byte for opv element
cr=szo/szc; % compression ratio

% reconstruction
opr=[0; cumsum(dop)];
tr=opr*dt;

opvr=opv/128;

tra=0:dt:max(tr); % time for interpolation

sw=interp1(tr,opvr,t,'pchip');

soundsc(sw,Fs);

plot(t,sw,'g-');

title(['compressed in ' num2str(cr) ' times']);
legend('original','optimums','reconstructed');


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 [MW] = hpb2MW(hpb)
% Convert power from boiler horsepower to megawatts.
% Chad A. Greene 2012
MW = hpb*9809.5e-6;


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


% Script demonstrating usage of the cbpdndlmd function.
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2017-04-29
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'Copyright' and 'License' files
% distributed with the library.

% Training images
S0 = zeros(512, 512, 2, 'single');
S0(:,:,1) = single(stdimage('lena.grey')) / 255;
S0(:,:,2) = single(stdimage('barbara.grey')) / 255;

%Reduce images size to speed up demo script
tmp = zeros(128, 128, 2, 'single');
for k = 1:size(S0,3),
  tmp(:,:,k) = imresize(S0(:,:,k), 0.25);
end
S0 = tmp;

% Filter input images and compute highpass images
npd = 16;
fltlmbd = 5;
[Sl, Sh] = lowpass(S0, fltlmbd, npd);

% Construct weight matrix and padded test image set
Shp = padarray(Sh, [7 7], 'post');
t = 0.5;
W = randn(size(Sh));
W(abs(W) > t) = 1;
W(abs(W) < t) = 0;
W = padarray(W, [7 7], 'post');
ShW = W .* Shp;

% Construct initial dictionary
D0 = zeros(8,8,32, 'single');
D0(3:6,3:6,:) = single(randn(4,4,32));

% Set up cbpdndl parameters
lambda = 0.05;
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 500;
opt.rho = 50*lambda + 0.5;
opt.sigma = size(Sh,3);
opt.AutoRho = 1;
opt.AutoRhoPeriod = 10;
opt.AutoSigma = 1;
opt.AutoSigmaPeriod = 10;
opt.XRelaxParam = 1.8;
opt.DRelaxParam = 1.8;

% Do standard dictionary learning and reconstruct
[D1, X1, optinf1] = cbpdndl(D0, ShW, lambda, opt);
DX1 = ifft2(bsxfun(@times, fft2(D1, size(X1,1), size(X1,2)), fft2(X1)), ...
           'symmetric');
Sr1 = squeeze(sum(DX1,3)) + padarray(Sl, [7 7], 'post');

% Do dictionary learning with mask decoupling and reconstruct
opt.W = W;
[D2, X2, optinf2] = cbpdndlmd(D0, ShW, lambda, opt);
DX2 = ifft2(bsxfun(@times, fft2(D2, size(X2,1), size(X2,2)), fft2(X2)), ...
           'symmetric');
Sr2 = squeeze(sum(DX2,3)) + padarray(Sl, [7 7], 'post');

% Display dictionaries
figure;
subplot(1,2,1);
imdisp(tiledict(D1));
title('Standard DL');
subplot(1,2,2);
imdisp(tiledict(D2));
title('DL with mask decoupling');

% Display reconstructions
figure;
subplot(2,2,1);
imdisp(Sr1(:,:,1));
title('Standard DL');
subplot(2,2,2);
imdisp(Sr2(:,:,1));
title('DL with mask decoupling');
subplot(2,2,3);
imdisp(Sr1(:,:,2));
title('Standard DL');
subplot(2,2,4);
imdisp(Sr2(:,:,2));
title('DL with mask decoupling');

% Plot functional value evolution
figure;
subplot(1,2,1);
semilogx(optinf1.itstat(:,2), 'LineWidth', 2);
ylim([15, 45]);
xlabel('Iterations');
ylabel('Functional value');
title('Standard DL');
subplot(1,2,2);
semilogx(optinf2.itstat(:,2), 'LineWidth', 2);
ylim([15, 45]);
xlabel('Iterations');
ylabel('Functional value');
title('DL with mask decoupling');


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 [Population,RankSolution] = EnvironmentalSelection(Population,N,alpha)

%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Mengjun Ming

    %% Parameter
    popSize    = length(Population);
    NumSeq     = (1:popSize)';
    RankConvg  = zeros(popSize,1);
    RankDivs   = zeros(popSize,1);

    %% Modify the infeasible solutions
    PopObj = Population.objs;
    PopCon = Population.cons;
    z      = min(PopObj,[],1);
    n      = max(PopObj,[],1);
    M      = length(z);
    [W,~]  = UniformPoint(N,M);
    [~,Region]     = min(pdist2(PopObj-z,W,'cosine'),[],2);
    PopObj_2       = PopObj;
    Infeasible_all = any(PopCon>0,2);
    if sum(Infeasible_all) ~= 0
        [~,Region_Fmax] = min(pdist2(n-z,W,'cosine'),[],2);
        PopObj_2(Infeasible_all,:) = repmat(n,sum(Infeasible_all),1)+sum(max(0,PopCon(Infeasible_all,:)),2)*W(Region_Fmax,:)/norm(W(Region_Fmax,:));
    end

    %% Non-dominated sorting
    CV       = sum(max(0,PopCon),2);
    Dominate = false(popSize);
    for i = 1 : popSize-1
        for j = i+1 : popSize
            if CV(i) < CV(j)
                Dominate(i,j) = true;
            elseif CV(i) > CV(j)
                Dominate(j,i) = true;
            else
                k = any(PopObj(i,:)<PopObj(j,:)) - any(PopObj(i,:)>PopObj(j,:));
                if k == 1
                    Dominate(i,j) = true;
                elseif k == -1
                    Dominate(j,i) = true;
                end
            end
        end
    end

    %% Calculate S(i)
    S = sum(Dominate,2);

    %% Calculate R(i)
    R = zeros(1,popSize);
    for i = 1 : popSize
        R(i) = sum(S(Dominate(:,i)));
    end
    FrontNo = R + 1;

    %% Calculate the crowding distance of each solution
    PopObj   = Population.objs;
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Distance = sort(Distance,2);
    CrowdDis = Distance(:,floor(sqrt(popSize)));

    %% Add a middle column
    MiddleLevel = zeros(popSize,1);

    %% Environmental selection -- convergence
    Next = FrontNo == 1;
    if sum(Next) <= N
        [~,indx_Convg] = sortrows([FrontNo',-CrowdDis]);
    elseif sum(Next) > N
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
        MiddleLevel(Temp(Del)) = 1;
        [~,indx_Convg] = sortrows([FrontNo',MiddleLevel,-CrowdDis]);
    end
    RankConvg(indx_Convg) = NumSeq;

    %% Environmental selection -- diversity
    FrontNo_D = ones(popSize,1);
    for i = 1:size(W,1)
        index = find(Region==i);
        if (~isempty(index))
            Objs_temp = PopObj_2(index,:);
            g_temp = sum((Objs_temp-z).*W(i,:),2);
            [~,index_FrontNo_D] = sort(g_temp);
            FrontNo_D(index(index_FrontNo_D)) = (1:length(g_temp))';
        end
    end
    FrontNo_D           = FrontNo_D+Infeasible_all*popSize;
    [~,indx_divs]       = sortrows([FrontNo_D,-CrowdDis]);
    RankDivs(indx_divs) = NumSeq;

    %% Population for next generation
    RankSolution = alpha*RankConvg+(1-alpha)*RankDivs;
    [~,Rank]     = sort(RankSolution);
    Population   = Population(Rank(1:N));
    RankSolution = 1 : N;
end
function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


%-------------------------------------------------------------------------
% This is a simulation demo to show multiple trackpaths for tracking the
% blood cell motion. In this demo, the diameter of the blood cell is 16
% pixels, so we use 9 trackpaths(in row 23) and the distance between each
% trackpath is 8 pixels(in row 25).
% Written by Yuan Chen,Nanjing university of aeronautic and astronautic(2010)
%-------------------------------------------------------------------------
clear all;
%% read the avi file, and calculate the mean image;
display('Reading and processing demo3.avi file...')
video=aviread('demo3.avi');             % read the avi file;
video = {video.cdata};                  % exracte the avi data(matrix);
z = 0;
for i=1:length(video);                  % change 'color' video to gray;
    video{i}=rgb2gray(video{i});
    z = z + double(video{i});
end
z = z/length(video);                    % the mean image;
[mz,nz] = size(z);
[mu1,mu2,v1x,v1y,v2x,v2y] = eigfunction(z,7,5);  % calculate the eigen values and eigen vectors of the mean image;

%% the trackpaths generation;
N = 9;                                  % THE number of trackpath can be set within [5,65];NOTE that,larger N will consume
                                        % much more processing times,we suggrest that N = 9, and no larger than 16;
t = 64/(N-1);                           % the distance between each trakcpath;
T = 64:t:128;                           % T are the radius of the trackpaths(the center(148,150)in the mean image z);
for k = 1:length(T)
    tmp = zeros(size(z));
    for i = 1:138
        for j = 1:300
            if sqrt((148-i)^2 + (150-j)^2 ) < T(k)+1 && sqrt((148-i)^2 + (150-j)^2 ) >= T(k);
               tmp(i,j) = 255;
            end
        end
    end
    tmp = im2bw(tmp);
    tmp = bwmorph(tmp,'thin',inf);
    trackpath{k} = tmp;
end
display([num2str(N),' trackpaths are selected to generate ',num2str(N),' ST images...Processing them may take several minutes'])
%% ST image generation, processing and traces extraction;
for i=1:size(trackpath,2)                                   % the number of the generated trackpath;
    map = [];
    [xx,yy] = startpoint(trackpath{i});                     % finding the starting point of a trackpath;
    ind_trackpath = ord_line_indx(trackpath{i},xx,yy);      % order the trackpath points;
    for j = 1:size(video,2)                                 % ST image generation;
        line = video{j}(ind_trackpath);
        map = [map;line];
    end
    map = map';
    Map{i} = map;                                           % store the ST image;
    display(['processing ST image',int2str(i)])
    [F,Theta,angle] = J4(map,2.5);                          % raw ST image enhanced by Jacob filter;
    R = nsf(F);                                             % applying noise supression function;
    K = angfilter(R,Theta,angle,20);                        % applying orientation filter function;
    th = graythresh(K);                                     % threshold for cell tracked trackpath;
    if th <= 0.01;                                          % threshold for none cell tracked trackpath;
        th = 0.01;
    end
    bw = im2bw(K,th);
    thin = bwmorph(bw,'thin',inf);                          % thinning extracted traces;
    trace{i} = bwareaopen(thin,10);                         % remove noise trace if its length smaller than 10 pixels;
end

%% calculate the trackpaths coordinate and grayscale;
h = fspecial('gaussian',7);
for i = 1:length(trackpath)                                 % denoise;
    Map_denoise{i} = conv2(Map{i},h,'same');
    Map_denoise{i} = conv2(Map_denoise{i},h,'same');
end
% -------find out the coordinate(X_trackpath,Y_trackpath) and grayscale of the trackpaths;
for i = 1:length(trackpath)
    [xx,yy] = startpoint(trackpath{i});                     % find the start coordinate of a trackpath;
    ind_trackpath = ord_line_indx(trackpath{i},xx,yy);      % search the trackpath's point from the start point orderly;
    for j = 1:length(ind_trackpath)
        [X_trackpath(j),Y_trackpath(j)] = ind2sub([mz,nz],ind_trackpath(j)); % change the index to (X,Y) coordinate;
    end
    Trackpath_value{i}(:,1) = X_trackpath';
    Trackpath_value{i}(:,2) = Y_trackpath';
    Trackpath_value{i}(:,3) = 0;
    [Y_trace,X_trace] = find(trace{i}==1);                  % find extracted trace coordinate;
    for k = 1:length(Y_trace)                               % store the grayscale of the trace;
        Trackpath_value{i}(Y_trace(k),3) = Map_denoise{i}(Y_trace(k),X_trace(k));
    end
    clear X_trackpath;clear Y_trackpath;
end
% -------backward mapping the extracted traces to the trackpaths;
for i = 1:length(Trackpath_value)
    for j = 1:size(Trackpath_value{i},1)
        z(Trackpath_value{i}(j,1),Trackpath_value{i}(j,2)) = Trackpath_value{i}(j,3);
    end
end
% figure,imshow(z,[]),title('The tracking state of trackpaths')

%% trackpaths alignment
k = round(length(T)/2);                                     % the centerline trackpath;
% -------turn all trackpaths and all ST images to the same length(or size) as the centreline trackpath and centreline ST image
% according to the eigen victor v1x,v1y;
for i = 1:length(T)
    p = findpoints(trackpath{k},trackpath{i},v1x,v1y);      % the corresponding relationship between the k and the i trackpaths.
    P{i} = round(smooth(p));
    if i~=k
        for j = 1:length(P{i})
            trace_aligned{i}(j,:) = trace{i}(P{i}(j),:);
            Points_aligned{i}(j,:) = Trackpath_value{i}(P{i}(j),:);
        end
    else
        trace_aligned{i} = trace{i};
        Points_aligned{i} = Trackpath_value{i};
    end
end

% -------base trace generation;
B_trace = 0;
for i = 1:length(trace_aligned)
    B_trace = B_trace + trace_aligned{i};
end
% figure,imshow(B_trace),title('the result of traces mapping')
B_trace = im2bw(B_trace);
B_trace = bwmorph(B_trace,'dilate');
B_trace = bwmorph(B_trace,'thin',inf);
% figure,imshow(B_trace),title('Base-trace')

%%  fusion of aligned points in trackpaths to calculate the trajectory;
l = 1;
for j = 1:size(Points_aligned{1},1)             % the number of the aligned points in the trackpath;
    Fusion_points = [];
    for i = 1:size(Points_aligned,2)            % the number of the trackpaths;
        if Points_aligned{i}(j,3) > 0           % >0 means the cell is tracked;
            X = Points_aligned{i}(j,1);         % X coordinate;
            Y = Points_aligned{i}(j,2);         % Y coordinate;
            V = Points_aligned{i}(j,3);         % grayvalue of (X,Y);
            Fusion_points = [Fusion_points,[X;Y;V]];
        end
    end
    if size(Fusion_points,2) > 2                % the cell is tracked by three or more than three trackpths;
        X_fused(l,1) = mean(Fusion_points(1,:));% the fused position is the center among the aligned points;
        Y_fused(l,1) = mean(Fusion_points(2,:));
        l = l+1;
    elseif size(Fusion_points,2) == 2           % the cell is tracked by two trackpaths(Eq.(15)in the paper);
        x1 = Fusion_points(1,1); x2 = Fusion_points(1,2);
        y1 = Fusion_points(2,1); y2 = Fusion_points(2,2);
        v1 = Fusion_points(3,1); v2 = Fusion_points(3,2);
        [X_fused(l,1),Y_fused(l,1)] = fusion2p(x1,y1,x2,y2,v1,v2,0.4); % the fused position calculation;
        l = l+1;
    elseif size(Fusion_points,2) ==1            % the cell is tracked by only one trackpath;
        X_fused(l,1) = Fusion_points(1,1);      % no fusion is needed;
        Y_fused(l,1) = Fusion_points(2,1);
        l = l+1;
    end
end

   X_trajectory = round(X_fused);               % calculated trajectory;
   Y_trajectory = round(Y_fused);
   X_smooth = round(smooth(X_fused,10));        % smooth process to prevent jagged trajectory;
   Y_smooth = round(smooth(Y_fused,10));
for i = 1:length(X_fused)
    z(X_trajectory(i),Y_trajectory(i)) = 255;
end
figure,imshow(z,[]);title(['Blood cell tracking by ',num2str(N), ' trackpaths(black line)'])
hold on;plot(Y_smooth,X_smooth,'r')

% -------the real trajectory of the cell;
x = 76:228;
y = 124*sin(pi/162*x+80.32);
y = round(-y+151);
hold on,plot(x,y);
h = legend('Tracked trajectory','Real trajectory',2);

%% calculating the error and variance;
for i = 1:length(X_trajectory)                  % by finding the closest points in the real trajectory to calculate the error;
    for j = 1:length(x)
        dis(j) = sqrt((X_trajectory(i) - y(j))^2 + (Y_trajectory(i) - x(j))^2);
        Error(i) = min(dis);
    end
end
ES = [mean(Error),std2(Error)];                 % mean error and standard deviation;
display(['The Error and Variance of the tracked tracjectory by using ',num2str(N),' trackpaths are'])
display([num2str(ES(1)) ' pixels and ' num2str(ES(2)),' pixels'])


x = uiuc_sample;
x = x(1:256, 1:256);

Wop = wavelet_factory_2d(size(x));
%%
[Sx, Ux] = scat(x, Wop);

%%
plot_meta(Sx);


function log_evidence = ldae_is_variants(words, topics, topic_prior, num_samples, variant, variant_iters)
%LDAE_IS_VARIANTS approximate evidence of LDA model, importance sampling with choice of q-distributions
%
% log_evidence = ldae_is_variants(words, topics, topic_prior[, num_samples=1000[, variant=3[, variant_iters=1]]]);
%
% Inputs:
%             words 1xNd
%            topics TxV each row is a distribution over a vocabulary of size V
%       topic_prior 1xT parameters of Dirichlet from which document topic vector is drawn
%       num_samples 1x1 default 1000
%           variant 1x1 1: prior, 2: q(z) = \prod p(z_n|w_n), 3: q(z) = \prod(z_n|w_n,hacky_pseudo_counts)
%                       Default is 3, as that seems to work best.
%     variant_iters 1x1 If variant needs iterative updates, use this number
%
% Outputs:
%     log_evidence  1x1

% Iain Murray, January 2009

[T, V] = size(topics);
Nd = length(words);

if ~exist('num_samples', 'var')
    num_samples = 1000;
end
if ~exist('variant', 'var')
    variant = 3;
end
if ~exist('variant_iters', 'var')
    variant_iters = 1;
end

topic_prior = topic_prior(:)';
topic_alpha = sum(topic_prior);
%topic_mean = topic_prior / topic_alpha;

if variant == 1
    % Importance sample from prior
    qstar = repmat(topic_prior', 1, Nd); % T x Nd
    qq = bsxfun(@rdivide, qstar, sum(qstar, 1));
else
    % Take w_n into account when picking z_n
    qstar = bsxfun(@times, topic_prior', topics(:, words)); % T x Nd
    qq = bsxfun(@rdivide, qstar, sum(qstar, 1));

    if variant == 3
        for i = 1:variant_iters
            % Now create pseudo-counts from qq and recompute qq using them
            pseudo_counts = bsxfun(@minus, topic_prior' + sum(qq, 2), qq);
            qstar = bsxfun(@times, pseudo_counts, topics(:, words)); % T x Nd
            qq = bsxfun(@rdivide, qstar, sum(qstar, 1));
        end
    end
end

% Draw samples from the q-distribution
samples = zeros(Nd, num_samples);
for n = 1:Nd
    samples(n, :) = discreternd(num_samples, qq(:, n))'; % Nd x num_samples
end

% Evaluate P(z, v) at samples and compare to q-distribution
Nk = histc(samples, 1:T, 1); % T x num_samples
log_pz = sum(gammaln(bsxfun(@plus, Nk, topic_prior')), 1) ...
        + gammaln(topic_alpha) - sum(gammaln(topic_prior)) ...
        - gammaln(Nd + topic_alpha); % 1 x num_samples
log_w_given_z = zeros(1, num_samples);
for n = 1:Nd
    log_w_given_z = log_w_given_z + log(topics(samples(n,:), words(n)))';
end
log_joint = log_pz + log_w_given_z;
%
log_qq = zeros(1, num_samples);
for n = 1:Nd
    log_qq = log_qq + log(qq(samples(n,:), n))';
end
log_weights = log_joint - log_qq;
log_evidence = logsumexp(log_weights(:)) - log(length(log_weights));


classdef TSLDirectionKey < directionColorKey
  % converts directions to rgb values

  methods

    function dM = TSLDirectionKey(varargin)
      dM@directionColorKey(varargin{:});
      dM.sym = dM.sym.Laue;
      dM.sR = dM.sym.fundamentalSector;

      if ismember(dM.sym.id,[2,5,8,11,18,21,24,27,35,42])
        warning('Not a topological correct colormap! Green to blue colorjumps possible');
      end
    end

    function rgb = direction2color(dM,h,varargin)
      % in TSL all fundamental sectors are colorized with white in the center

      % project to fundamental region
      h = h.project2FundamentalRegion(dM.sym);

      % this should become white if not stated differently
      center = dM.sR.center;

      % compute angle of the points "sh" relative to the center point "center"
      % this should be between 0 and 1
      v = dM.sR.vertices;
      if ~isempty(v)
        v = v(1);
      else
        v  = dM.sym.aAxisRec;
      end
      [radius,rho] = polarCoordinates(dM.sR,h,center,v);

      % white center
      radius = 0.5+radius./2;

      % compute the color vector on the sphere
      v = vector3d('rho',rho,'theta',radius.*pi);

      % post processing of the color vector
      % by default we have white at the z, black at the -z, red
      % at x and green and blue at 120 and 240 degree accordingly
      % post rotate the color
      v = dM.colorPostRotation * v;

      % compute rgb values
      rgb = ar2rgb(mod(v.rho./ 2 ./ pi,1),v.theta./pi,get_option(varargin,'grayValue',1),'noHueCorrection');

    end
  end

end


function [x_pred, y_pred]= convert_trans_and_scale(pred_point_raw, mid_point, scale)
   % 1. The input heatmap is 248 by 248, which is rescaled from a crop, introducing
   %    a scale
   % 2. The crop comes from the original image, and introduce a translation
   % Summary: This script is to do the backwards mapping, back to the original position.
   % (1) First, find the position in the cropped image
   % (2) Second, find the position in the original image

   % Use (scale) for the scale. [180, 280] is current value.
   % Consult [gen_cropped_test_images.m]. Make sure use same scale.
   % Officially, scale is w.r.t. 200 px height
   human_bbox_wid = 180 * scale;
   human_bbox_ht = 280 * scale;

   % Prepare [x_min, y_min] for translation
   x_min = mid_point(1) - human_bbox_wid/2.0;
   y_min = mid_point(2) - human_bbox_ht/2.0;

   % Based on translation, scale, and output, find input
   [x_pred, y_pred] = find_pos_for_org_image(pred_point_raw, human_bbox_wid, human_bbox_ht, x_min, y_min);
end

function [x0, y0] = find_pos_for_org_image(pos_248, human_bbox_wid, human_bbox_ht, x_min, y_min)
    [x1, y1] = find_pos_for_cropped_image(pos_248, human_bbox_wid, human_bbox_ht);

    x0 = x1 + x_min;
    y0 = y1 + y_min;

    pos_org = [x0, y0];
end

function [x1, y1] = find_pos_for_cropped_image(pos_248, human_bbox_wid, human_bbox_ht)
   x2 = pos_248(1);
   y2 = pos_248(2);

   x1 = int64((x2 * human_bbox_wid)/248.0);
   y1 = int64((y2 * human_bbox_ht)/248.0);

   pos_cropped = [x1, y1];
end


function b_im = Blur_IM( im, H )

[h w ch]  =  size(im);

if ch==3
    m1  = im(:,:,1);
    m2  = im(:,:,2);
    m3  = im(:,:,3);
    b_im(:,:,1)  =  reshape( H*m1(:), h, w );
    b_im(:,:,2)  =  reshape( H*m2(:), h, w );
    b_im(:,:,3)  =  reshape( H*m3(:), h, w );
else
    b_im  =  reshape( H*im(:), h, w );
end


classdef H0Kernel < Kernel
    properties
        k;
        C; % Such that G(r) = C*besselh(0,1,k*r)
        % Hankel function of the first kind.
    end
    methods
        function[this] = H0Kernel(kk,CC)
            if nargin == 0
                kk = 1;
            end
            if nargin <= 1
                CC = 1;
            end
            this.k = kk;
            this.C = CC;
            Jk = J0Kernel(kk);
            Yk = Y0Kernel(kk);
            modelKern = this.C*(Jk + 1i*Yk);
            this.func = modelKern.func;
            this.der = modelKern.der;
            this.scalFunc = modelKern.scalFunc;
            this.lim0 = this.C*(Jk.lim0 + 1i*Yk.lim0);
        end
        function[out] = dilatation(this,lambda)
            lim0keep = this.lim0;
            out = H0Kernel(this.k*lambda,this.C);
            out.lim0 = lim0keep;
        end
        function[out] = mtimes(this,mu)
            if isa(this,'Kernel')
                assert(and(isa(mu,'double'),isscalar(mu)));
                out = H0Kernel(this.k,this.C*mu);
            else
                out = mtimes(mu,this);
            end
        end
        function[rq] = radialQuadKernel(this,a,tol,varargin)
            modelKern = J0Kernel(this.k) + 1i*Y0Kernel(this.k);
            rqTemp = modelKern.radialQuadKernel(a,tol/abs(this.C),varargin{:});
            rq = this.C*rqTemp;
        end
    end
end


function N = mtimes(K, M)
% N = mtimes(K, M)
%
%  kronMatrix multiplication;
%     multiply a kronMatrix by a matrix, a vector, or by
%     another kronMatrix (if possible),
%
%

% 9/2002 L. Perrone
% written for new kronMatrix class

% 6/2003 J. Nagy
% some minor modifications to incorporate new subsref.m capabilities

if (isa(K, 'kronMatrix'))
   if isa(M, 'double')
      N = left_mtimes(K, M);
   elseif isa(M,'kronMatrix')
      if length(M) == 1
         sizeMa = size(M.a{1});
         sizeMb = size(M.b{1});
         l = length(K);
         Anew = cell(l,1);
         Bnew = cell(l,1);
         for i = 1:l
           sizeKa = size(K.a{i});
           sizeKb = size(K.b{i});
           if sizeKa(2)==sizeMa(1) & sizeKb(2)==sizeMb(1)
             Anew{i} = K.a{i} * M.a{1};
             Bnew{i} = K.b{i} * M.b{1};
           else
             error('Kron factors must be of compatible sizes for multiplication')
           end
         end % end the for i=1:l loop
         N = kronMatrix(Anew,Bnew);
      elseif length(K) == 1
         sizeKa = size(K.a{1});
         sizeKb = size(K.b{1});
         l = length(M);
         Anew = cell(l,1);
         Bnew = cell(l,1);
         for i = 1:l
           sizeMa = size(M.a{i});
           sizeMb = size(M.b{i});
           if sizeKa(2)==sizeMa(1) & sizeKb(2)==sizeMb(1)
             Anew{i} = K.a{1} * M.a{i};
             Bnew{i} = K.b{1} * M.b{i};
           else
             error('Kron factors must be of compatible sizes for multiplication')
           end
         end % end the for i=1:l loop
         N = kronMatrix(Anew,Bnew);
      else
         error('This currently only works if one of the kronMatrix objects has one term in it''s sum.')
      end
   else % M isn't a 'double' or a 'kronMatrix'
      error('Wrong input arguments')
   end % end the case when K is a 'kronMatrix'

elseif isa(K,'double') & isa(M,'kronMatrix')
   N = right_mtimes(K, M);
else
   error('Wrong input arguments')
end


% Script demonstrating usage of the bpdndl function.
%
% 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 'Copyright' and 'License' files
% distributed with the library.

% Training images
S0 = zeros(512, 512, 5);
S0(:,:,1) = single(stdimage('lena.grey')) / 255;
S0(:,:,2) = single(stdimage('barbara.grey')) / 255;
S0(:,:,3) = single(stdimage('kiel.grey')) / 255;
S0(:,:,4) = single(rgb2gray(stdimage('mandrill'))) / 255;
tmp = single(stdimage('man.grey')) / 255;
S0(:,:,5) = tmp(101:612, 101:612);

%Reduce images size to speed up demo script
tmp = zeros(256, 256, 5);
for k = 1:size(S0,3),
  tmp(:,:,k) = imresize(S0(:,:,k), 0.5);
end
S0 = tmp;

% Extract all 8x8 image blocks, reshape, and subtract block means
SB = imageblocks(S0, [8 8]);
SB = reshape(SB, size(SB,1)*size(SB,2), size(SB,3));
S = bsxfun(@minus, SB, mean(SB, 1));

% Construct initial dictionary
D0 = randn(size(S,1), 64);

% Set up bpdndl parameters
lambda = 0.2;
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 1000;
opt.rho = 50*lambda + 0.5;
opt.sigma = size(S,2)/200;
opt.AutoRho = 1;
opt.AutoRhoPeriod = 10;
opt.RhoRsdlRatio = 2;
opt.RhoScaling = 5;
opt.AutoRhoScaling = 1;
opt.AutoSigma = 1;
opt.AutoSigmaPeriod = 10;
opt.XRelaxParam = 1.8;
opt.DRelaxParam = 1.8;

% Do dictionary learning
[D, X, optinf] = bpdndl(D0, S, lambda, opt);

% Display learned dictionary
figure;
imdisp(tiledict(D, [8 8]));

% Plot functional value evolution
figure;
plot(optinf.itstat(:,2));
xlabel('Iterations');
ylabel('Functional value');


function [coef, res, wres, ymoco] = robust_ar_fit( y, Pmax )
    [~, res] = ar_fit(y, Pmax);

    w = wfun(res);
    [coef, ~] = ar_fit(w.*y, Pmax);

    res     = filter([1; -coef(2:end)], 1, y-coef(1));
    wres    = filter([1; -coef(2:end)], 1, w.*y-coef(1));

    ymoco   = filter(1, [1; -coef(2:end)], w.*res);
end

function w = wfun(r)
    s = mad(r, 0) / 0.6745;
    r = r/s/4.685;

    w = (1 - r.^2) .* (r < 1 & r > -1);
end


function [Gx,Gy] = gradient2( I )
% Compute numerical gradients along x and y directions.
%
% For 2D arrays identical to Matlab's gradient() with a spacing value of
% h=1 but ~10-20x faster (due to mexed implementation). Like gradient(),
% computes centered derivatives in interior of image and uncentered
% derivatives along boundaries. For 3D arrays computes x and y gradient
% separately for each channel and concatenates the results.
%
% This code requires SSE2 to compile and run (most modern Intel and AMD
% processors support SSE2). Please see: http://en.wikipedia.org/wiki/SSE2.
%
% USAGE
%  [Gx,Gy] = gradient2( I )
%
% INPUTS
%  I      - [hxwxk] input k channel single image
%
% OUTPUTS
%  Gx     - [hxwxk] x-gradient (horizontal)
%  Gy     - [hxwxk] y-gradient (vertical)
%
% EXAMPLE
%  I=single(imread('peppers.png'))/255;
%  tic, [Gx1,Gy1]=gradient(I,1); toc
%  tic, [Gx2,Gy2]=gradient2(I); toc
%  isequal(Gx1,Gx2), isequal(Gy1,Gy2)
%
% See also gradient, gradientMag
%
% Piotr's Image&Video Toolbox      Version 3.00
% Copyright 2012 Piotr Dollar & Ron Appel.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

[Gx,Gy]=gradientMex('gradient2',I);