clear all;
close all;
clc;

display('Nth (3) order Reconstruction');
N = 3;                  % Nth order nonuniform sampling
TQ = 1;Fs = 1/TQ;                 % Nyquist Period

T = [2*TQ 3*TQ 6*TQ];      % Decimation Periods
K = 0.5*lcm(2*T(1), 2*T(2)); K = 0.5*lcm(2*K, 2*T(3))/TQ;
capT = K*TQ; M = capT./T;
capM = lcm(M(1), M(2)); capM = lcm(capM, M(3));
excess = ceil((K-1)/capM);
maxf = K/(excess*capM+1);
TQ1 = maxf*TQ;
K1 = K/maxf;

ML = 400; % number of slices
w_c = 0.85;
NS = 100;  % Number of Sinusoids

LF = capM*2*K1*(1:10)+1;  %359,159,239          % min length of LF should be capM*2*K1

std = 1e-1;
taus = [0 1.1+std*randn 2.2+std*randn]*TQ;
tausI = sort([taus(1) taus(2) taus(3) T(1)+taus(1) T(2)+taus(2) 2*T(1)+taus(1)]);
% display(tausI);

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);
inputN = zeros(1,ML*K1);
for k = 1:NS
  input = input + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K-1)*TQ+Phi(k));
  inputN = inputN + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K1-1)*TQ1+Phi(k));
end;

tauI = zeros(K,ML);
for p = 1:K
    tauI(p,:) = tausI(p)+(0:ML-1)*capT;
end;

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

tauPr = zeros(K,ML);
for p = 1:K
    tauPr(p,:) = -tausI(p)+(0:ML-1)*capT;
end;
x1p = zeros(K,ML);
for k = 1:NS
    x1p = x1p + Amp(k)*sin(2*pi*Frq(k)*tauPr+Phi(k));
end;

timeP = zeros(size(LF));
timeE = zeros(size(LF));
timeI = zeros(size(LF));
timeV = zeros(size(LF));
timePr = zeros(size(LF));
timeJ = zeros(size(LF));

MC_runs = 1;
for rrr = 1:length(LF)
display(rrr);
n = -(LF(rrr)-1)/2:1:(LF(rrr)-1)/2;

for tt = 1:MC_runs

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filterbank Reconstruction of Bandlimited Signals from Nonuniform and
% Generalized Samples
% Authors: Y C Eldar and A V Oppenheim
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y = zeros(N,ML*K1);
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;
    LFE = M(p)*84*rrr+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
    nE = -(LFE-1)/2:1:(LFE-1)/2;

    tic
    y1 = upsample(x1,K1);
    h = sinc((nE/K1)-(taus(p)/T(p))).*kaiser_mine1(LFE,18,-K1*(taus(p)/T(p)));

%%%%%%%%%%%%%%%%%%%%%%%%%%% Implementation 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     G = M(setdiff((1:N),p));
%     F = taus(setdiff((1:N),p));
%     temp1 = G(1)-G(2);
%     temp2 = G(1)+G(2);
%     bb = zeros(2*(K-M(p))+1,M(p));
%     if temp1~=0
%         for l = 0:M(p)-1
%             c = 0.5*cos(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
%             s = -0.5*sin(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
%             bb(1+temp2-temp1,l+1) = 0.5*(c+1i*s);
%             bb(1+temp2+temp1,l+1) = conj(bb(1+temp2-temp1,l+1));
%         end;
%     else
%         bb(1+temp2,1:M(p)) = 0.5*cos(pi*G(1)*(F(2)-F(1))/capT);
%     end;
%     for l = 0:M(p)-1
%         c = -0.5*cos(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
%         s = 0.5*sin(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
%         bb(1+temp2-temp2,l+1) = 0.5*(c+1i*s);
%         bb(1+temp2+temp2,l+1) = conj(bb(1+temp2-temp2,l+1));
%     end;
%     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));
%             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/(K1*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 2 %%%%%%%%%%%%%%%%%%%%%%%%%%%
%     G = M(setdiff((1:N),p));
%     F = taus(setdiff((1:N),p));
%     temp1 = G(1)-G(2);
%     temp2 = G(1)+G(2);
%     bb = zeros(2*(K-M(p))+1,M(p));
%     if temp1~=0
%         for l = 0:M(p)-1
%             c = 0.5*cos(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
%             s = -0.5*sin(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
%             bb(1+temp2-temp1,l+1) = 0.5*(c+1i*s);
%             bb(1+temp2+temp1,l+1) = conj(bb(1+temp2-temp1,l+1));
%         end;
%     else
%         bb(1+temp2,1:M(p)) = 0.5*cos(pi*G(1)*(F(2)-F(1))/capT);
%     end;
%     for l = 0:M(p)-1
%         c = -0.5*cos(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
%         s = 0.5*sin(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
%         bb(1+temp2-temp2,l+1) = 0.5*(c+1i*s);
%         bb(1+temp2+temp2,l+1) = conj(bb(1+temp2-temp2,l+1));
%     end;
%     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));
%             end;
%         end;
%     end;
%     AE = diag(aaa);
%     BE = bb.';
%
%     dim = K-M(p); w = -dim:1:dim;
%     FE = exp(1i*(pi/(K1*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 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*TQ1/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);
    timeE(rrr) = timeE(rrr)+toc;
end;
y = real(sum(y,1));
x = inputN;
% y = y(160:end);
% x = x(160:end);
serE = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A realization of Digital Filter Banks for Reconstruction of Uniformly
% sampled signals from nonuniform samples
% Authors: Itami, Watanabe, Nishihara
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
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.25*(sin(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)-0.5*sin(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)-0.5*cos(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)-0.5*sin(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(3+1)+tausI(4+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)+0.5*sin(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(3+1)+tausI(4+1))/capT)-0.5*cos(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(3+1)+tausI(4+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)+0.5*sin(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(1+1)+tausI(2+1))/capT)*sin(pi*(tausI(3+1)+tausI(4+1))/capT));
s = 0.25*(cos(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)+0.5*cos(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)-0.5*sin(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(4+1)-tausI(3+1))/capT)+0.5*cos(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(3+1)+tausI(4+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)+0.5*cos(pi*(-tausI(5+1))/capT)*cos(pi*(tausI(1+1)+tausI(2+1))/capT)*cos(pi*(tausI(3+1)+tausI(4+1))/capT)-0.5*sin(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(3+1)+tausI(4+1))/capT)*cos(pi*(tausI(2+1)-tausI(1+1))/capT)+0.5*cos(pi*(-tausI(5+1))/capT)*sin(pi*(tausI(1+1)+tausI(2+1))/capT)*sin(pi*(tausI(3+1)+tausI(4+1))/capT));
b(5,1) = 0.5*(c+1i*s);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

y1=upsample(x11.',K1).';

k = -(K-1):1:(K-1);
m = (0:1:(2*K1-1))';
F = exp(1i*(pi/K1).*kron(m,k));

y = zeros(K,size(y1,2));
for r = 1:K
    tempI = a(r)*y1(r,:);
%     tempI2 = F*b(:,r)*a(r);
    tempI2 = F*b(:,r);
%     y2 = tempI2*y1(r,:);
    y2 = tempI2*tempI;
%     y2 = F*b(:,r)*a(r)*y1(r,:);
    h = sinc((n/K1)-tausI(r)/capT).*kaiser_mine1(LF(rrr),18,-tausI(r)/TQ1);
    for i=1:2*K1
            h1 = upsample(downsample(h,2*K1,i-1),2*K1);
%             y2(i,:) = filter(h1,1,y2(i,:));
%             y2(i,:) = filter([zeros(1,i-1),1],1,y2(i,:));%,zeros(1,2*K-i)
            temp = filter([zeros(1,i-1),1],1,h1);
            y2(i,:) = filter(temp,1,y2(i,:));
    end;
    y(r,:) = sum(y2,1);
end;
y = -real(sum(y,1));
timeI(rrr) = timeI(rrr)+toc;
delayI = (LF(rrr)-1)/2;
x=inputN(1:end-delayI);
y=y(1+delayI:end);
% y = y(160:end);
% x = x(160:end);
serI = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of Nonuniformly Sampled Band-Limited Signals
% Using a Differentiator-Multiplier Cascade
% Authors: Stefan Tertinek and Christian Vogel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = tausI-TQ*(0:K-1);
rr = r(mod((0:ML*K-1),K)+1);
x1 = reshape(x11,1,K*size(x11,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)|')
tic
Hd = firpm(LF(rrr)-1,[0 w_c],[0 w_c*pi],'differentiator');
delayV = (LF(rrr)-1)/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
timeV(rrr) = timeV(rrr)+toc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y = real(y3(1+6*delayV:end));
x = input(1:end-6*delayV);
% y = y(160:end);
% x = x(160:end);
serV = 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
H = zeros(K,LF(rrr));
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)).*kaiser_mine1(LF(rrr),18,-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
yb = upsample(x1p.',K).';
for i = 1:K
    yb(i,:) = filter(FR(i,:),1,yb(i,:));
end;
y = sum(yb,1);
timePr(rrr) = timePr(rrr)+toc;
delayPr = (size(FR,2))/2+K-1;
x=input(1:end-delayPr);
y=y(1+delayPr:end);
% y = y(160:end);
% x = x(160:end);
serPr = 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r = r.';
x1 = reshape(x11,1,size(x11,2)*K);
tic
w_o = w_c*pi*TQ;
hJ = zeros(K,LF(rrr));
C = zeros(1,LF(rrr));
Nt = (LF(rrr)-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(rrr),LF(rrr));
    for k = 1:LF(rrr)
        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;

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);
timeJ(rrr) = timeJ(rrr)+toc;
delayJ = (size(hJ,2)-1)/2;
y = real(y(1+delayJ:end));
x = input(1:end-delayJ);
% y = y(160:end);
% x = x(160:end);
serJ = 20*log10(norm(x,2)/norm(y-x,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reconstruction of N-th order nonuniformly sampled bandlimited signals
% using digital filter banks
% Authors: S. K. Sindhi, K. M. M. Prabhu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xp = zeros(N,ML*K1);
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;
    tic
    m = (0:1:M(p)-1)'; lemda = 0:1:M(p)-1;
    W = exp(1i*(2*pi/M(p)).*kron(m,lemda)); % m*lemda

    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));
            end;
        end;
    end;
    A = diag(aaa); %display(A);

    G = M(setdiff((1:N),p));
    F = taus(setdiff((1:N),p));
    temp1 = G(1)-G(2);
    temp2 = G(1)+G(2);
    bb = zeros(2*(K-M(p))+1,M(p));
    if temp1~=0
        for l = 0:M(p)-1
            c = 0.5*cos(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
            s = -0.5*sin(pi*(l*T(p)*temp1+G(2)*F(2)-G(1)*F(1))/capT);
            bb(1+temp2-temp1,l+1) = 0.5*(c+1i*s);
            bb(1+temp2+temp1,l+1) = conj(bb(1+temp2-temp1,l+1));
        end;
    else
        bb(1+temp2,1:M(p)) = 0.5*cos(pi*G(1)*(F(2)-F(1))/capT);
    end;
    for l = 0:M(p)-1
        c = -0.5*cos(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
        s = 0.5*sin(pi*(l*T(p)*temp2-G(2)*F(2)-G(1)*F(1))/capT);
        bb(1+temp2-temp2,l+1) = 0.5*(c+1i*s);
        bb(1+temp2+temp2,l+1) = conj(bb(1+temp2-temp2,l+1));
    end;
    B = bb;% display(B);

    y1 = upsample(x1,K1);
    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*K1-1))';
        Fshift = exp(1i*(pi/K1).*kron(rP,w));   % r*w
        mtemp = A*W;
        mtemp = B*mtemp;
        mtemp = Fshift*mtemp;
        Htemp = mtemp*W(:,lemda+1);

        h = sinc((n*TQ1/T(p))+(lemda/K1)-(taus(p)/T(p))).*kaiser_mine1(LF(rrr),18,(lemda/M(p))-(taus(p)/TQ1));
        h1 = zeros(2*K1, length(h)+2*K1-1);
        for i = 2:2*K1
            h1(i,:) = [filter([zeros(1,i-1),1],1,upsample(downsample(h,2*K1,i-1),2*K1)) zeros(1,2*K1)]*Htemp(i);
        end;
        h1(1,:) = upsample(downsample(h,2*K1),2*K1)*Htemp(1);
        h1 = sum(h1,1);
        xlemda(lemda+1,:) = filter(h1,1,y1(lemda+1,:));
    end;
    xp(p,:) = sum(xlemda,1)/M(p);
    timeP(rrr) = timeP(rrr)+toc;
end;
y = sum(xp,1);
delayP = (length(h)-1)/2;
y = y(1+delayP:end);
x = inputN(1:end-delayP);
% y = y(160:end);
% x = x(160:end);
serP = 20*log10(norm(x,2)/norm(y-x,2));
end;
end;
timeP = timeP/MC_runs;
timeE = timeE/MC_runs;
timeI = timeI/MC_runs;
timeV = timeV/MC_runs;
timePr = timePr/MC_runs;
timeJ = timeJ/MC_runs;

figure();hold on;
plot(LF,timeJ,'kp-','LineWidth',2);
plot(LF,timePr,'ko-','LineWidth',2);
plot(LF,timeV,'ks-','LineWidth',2);
plot(LF,timeP,'kd-','LineWidth',2);
plot(LF,timeI,'k>-','LineWidth',2);
plot(LF,timeE,'k+-','LineWidth',2);
legend('Johansson','Prendergast','Tertinek','Proposed','Itami','Eldar');
xlabel('Filter length','fontsize',14,'fontweight','b');
ylabel('Time in seconds','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

% temp = (LF.^3);%+(K*LF*ML*K);
% temp = temp/max(temp);
% timePrI = timePr(rrr)*temp;
%
% temp = (LF.^3);%+(K*(LF+1)*ML*K);
% temp = temp/max(temp);
% timeJI = timeJ(rrr)*temp;
%
% temp = (LF.^2);%+(6*LF*ML*K);
% temp = temp/max(temp);
% timeVI = timeV(rrr)*temp;
% plot(LF,timePrI,'k--','LineWidth',2);
% plot(LF,timeJI,'k--','LineWidth',2);
% plot(LF,timeVI,'k--','LineWidth',2);

figure();hold on;
plot(LF,timeJ,'kp-','LineWidth',2);
temp = (LF.^3);%+(K*(LF+1)*ML*K);
temp = temp/max(temp);
timeJI = timeJ(rrr)*temp;
plot(LF,timeJI,'k--','LineWidth',2);
legend('Johansson','Ideal curve');
xlabel('Filter length','fontsize',14,'fontweight','b');
ylabel('Time in seconds','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

figure();hold on;
plot(LF,timePr,'ko-','LineWidth',2);
temp = (LF.^3);%+(K*LF*ML*K);
temp = temp/max(temp);
timePrI = timePr(rrr)*temp;
plot(LF,timePrI,'k--','LineWidth',2);
legend('Prendergast','Ideal curve');
xlabel('Filter length','fontsize',14,'fontweight','b');
ylabel('Time in seconds','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

figure();hold on;
plot(LF,timeV,'ks-','LineWidth',2);
temp = (LF.^2);%+(6*LF*ML*K);
temp = temp/max(temp);
timeVI = timeV(rrr)*temp;
plot(LF,timeVI,'k--','LineWidth',2);
legend('Tertinek','Ideal curve');
xlabel('Filter length','fontsize',14,'fontweight','b');
ylabel('Time in seconds','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')

display(serI)
display(serV)
display(serPr)
display(serJ)
display(serP)
display(serE)

% LF = 84*(1:30)+1;      % length of LF should be Multiple of LCM{M(p)}*2*K
% timeJ = [timeJ,zeros(1,length(1:length(LF)-10))];
% for rrr = 11:length(LF)
% display(rrr);
% n = -(LF(rrr)-1)/2:1:(LF(rrr)-1)/2;
% for tt = 1:MC_runs
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Reconstruction of Periodically Nonuniformly Sampled Bandlimited Signals
% % Using Time-Varying FIR Filters
% % Authors: H. Johansson and Per Lowenborg
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% r = tausI-TQ*(0:K-1);
% r = r.';
% x1 = reshape(x11,1,size(x11,2)*K);
% tic
% w_o = w_c*pi*TQ;
% hJ = zeros(K,LF(rrr));
% C = zeros(1,LF(rrr));
% Nt = (LF(rrr)-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(rrr),LF(rrr));
%     for k = 1:LF(rrr)
%         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;
%
% 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);
% timeJ(rrr) = timeJ(rrr)+toc;
% delayJ = (size(hJ,2)-1)/2;
% y = real(y(1+delayJ:end));
% x = input(1:end-delayJ);
% % y = y(160:end);
% % x = x(160:end);
% serJ = 20*log10(norm(x,2)/norm(y-x,2));
% end;
% end;
% timeJ(11:length(LF)) = timeJ(11:length(LF))/MC_runs;
% figure();hold on;
% plot(LF,timeJ,'kp-','LineWidth',2);
% temp = (LF.^3);%+(K*(LF+1)*ML*K);
% temp = temp/max(temp);
% timeJI = timeJ(rrr)*temp;
% plot(LF,timeJI,'k--','LineWidth',2);
% legend('Johansson','Prendergast','Tertinek','Proposed','Itami','Eldar');
% xlabel('Filter length','fontsize',14,'fontweight','b');
% ylabel('Time in seconds','fontsize',14,'fontweight','b');
% grid on;box on;
% set(gca,'fontsize',14,'fontweight','b')

% 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;


% VL_FISHER    Fisher vector feature encoding
%   ENC = VL_FISHER(X, MEANS, COVARIANCES, PRIORS) computes the Fisher
%   vector encoding of the vectors X relative to the Gaussian mixture
%   model with means MEANS, covariances COVARIANCES, and prior mode
%   probabilities PRIORS.
%
%   X has one column per data vector (e.g. a SIFT descriptor), and
%   MEANS and COVARIANCES one column per GMM component (covariance
%   matrices are assumed diagonal, hence these are simply the variance
%   of each data dimension). PRIORS has size equal to the number of
%   GMM components. All data must be of the same class, either SINGLE
%   or DOUBLE.
%
%   ENC is a vector of the same class of X of size equal to the
%   product of the data dimension and the number of components.
%
%   By default, the standard Fisher vector is computed. VL_FISHER()
%   accepts the following options:
%
%   Normalized::
%     If specified, L2 normalize the Fisher vector.
%
%   SquareRoot::
%     If specified, the signed square root function is applied to
%     ENC before normalization.
%
%   Improved::
%     If specified, compute the improved variant of the Fisher
%     Vector. This is equivalent to specifying the Normalized and
%     SquareRoot options.
%
%   Fast::
%     If specified, uses slightly less accurate computations but
%     significantly increase the speed in some cases (particularly
%     with a large number of Gaussian modes).
%
%   Verbose::
%     Increase the verbosity level (may be specified multiple times).
%
%   See: Fisher vectors, VL_HELP().

% Authors: David Novotny, Andrea Vedaldi

% Copyright (C) 2013 David Novotny and Andrea Vedaldi
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function Gxy = ffmGreenKernel(X,Y,green,k)
%+========================================================================+
%|                                                                        |
%|         OPENFFM - LIBRARY FOR FAST AND FREE MEMORY CONVOLUTION         |
%|           openFfm is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : ffmGreenKernel.m                              |
%|    #    |   VERSION    : 0.6                                           |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Green kernel computation using string         |
%|  `---'  |                definition                                    |
%+========================================================================+

% Security
if (size(X,2) ~= 3) || (size(Y,2) ~= 3)
    error('ffmGreenKernel.m : unavailable case')
end
if isempty(k)
    k = 0;
end

% Distances between particles
Rxy = sqrt( ...
    (X(:,1) - Y(:,1)).^2 + ...
    (X(:,2) - Y(:,2)).^2 + ...
    (X(:,3) - Y(:,3)).^2 );

% For empty wave-number
if isempty(k)
    k = 0;
end

% Green kernel definition
if strcmp(green,'[1/r]')
    Gxy = 1./Rxy;

elseif strcmp(green,'[exp(ikr)/r]')
    Gxy = exp(1i*k*Rxy)./Rxy;

elseif strcmp(green(1:end-1),'gradx[1/r]')
    j = str2double(green(end));
    Gxy = - (X(:,j)-Y(:,j)) ./ (Rxy.^3);

elseif strcmp(green(1:end-1),'grady[1/r]')
    j = str2double(green(end));
    Gxy = (X(:,j)-Y(:,j)) ./ (Rxy.^3);

elseif strcmp(green(1:end-1),'gradx[exp(ikr)/r]')
    j = str2double(green(end));
    Gxy = (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) .* ...
        (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green(1:end-1),'grady[exp(ikr)/r]')
    j = str2double(green(end));
    Gxy = - (1i*k - 1./Rxy) .* exp(1i*k.*Rxy) .* ...
        (X(:,j)-Y(:,j)) ./ (Rxy.^2);

elseif strcmp(green(1:end-2),'[ij/r+rirj/r^3]')
    i = str2double(green(end-1));
    j = str2double(green(end));
    Gxy = (i==j)./Rxy + (X(:,i)-Y(:,i)).*(X(:,j)-Y(:,j))./(Rxy.^3);

elseif strcmp(green(1:end-3),'[rirjrk/r^5]')
    i = str2double(green(end-2));
    j = str2double(green(end-1));
    k = str2double(green(end));
    Gxy = (X(:,i)-Y(:,i)).*(X(:,j)-Y(:,j)).*(X(:,k)-Y(:,k))./(Rxy.^5);

else
    error('Error in ffmGreenKernel.m : unknown green kernel')
end

% Singularity
if strcmp(green,'[exp(ikr)/r]')
    Gxy(Rxy<1e-8) = 0 + 1i*k;
else
    Gxy(Rxy<1e-8) = 0;
end
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function  [Jc,para,dJ,H] = LDDMMobjFctn(T,Rc,omega,m,beta,M,wRef,xc,vc)
%
% Objective Function for LDDMM using Lagrangian PDE solver
%
% computes J(vc) = D(T(yc(vc)),Rc) + S(vc - vRef), where
%
% vc       - is a velocity field (stationary or instationary)
% yc(vc)   = trafo(wc,xc) is obtained by tracing characteristics using RK4
%            scheme
% Tc       = T(yc) = imgModel(T,omega,yc),
% D(Tc,Rc) = distance(Tc,Rc,omega,m)
% S        = regularizer, e.g., S(uc) = 0.5*uc'*B*uc
% vRef     = reference guess for velocities
%
% For more details see the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% Input:
%   T      - data for template image, Tc = imgModel(T,omega,yc)
%   Rc     - reference image on grid, Rc = imgModel(R,omega,xc)
%   omega  - representation of computational domain
%   m      - discretization size
%   vRef   - reference velocity
%   xc     - discretization of Omega
%   omegaV - computational domain for velocity field (can be larger)
%   mV     - discretization size for velocities
%   N      - number of time steps for RK4 scheme
%   vc     -  current velocities
%
% Output:
%  Jc      - current function value J(vc)
%  para    - struct {Tc=T(y(vc)), Rc, omega, m, yc=y(vc,xc), Jc}, for plots
%  dJ      - gradient of J
%  H       - approximation to Hessian of J
%
% see also ELDDMM_Hand2D
%==============================================================================
function [Jc,para,dJ,H] = LDDMMobjFctn(T,Rc,omega,m,vRef,xc,omegaV,mV,N,vc)

para = struct([]);
if nargin == 0,
  help(mfilename);
  runMinimalExample;
  return;
elseif ~exist('vc','var') || isempty(vc),
  % if wc is not an input argument, reports status
  if nargout == 1, Jc = 'LDDMM';  return; end;
  % report current settings
  dimstr  = @(m) sprintf('[%s]',sprintf(' %d',m));
  vc      = trafo('w0');
  nt      = regularizer('get','nt');
  fprintf('Large Deformation Diffeomorphic Mapping (LDDMM):\n');
  fprintf('   J(vc) = D(T(yc(vc)),Rc) + S(vc-vRef) != min\n');
  fprintf('  %20s : %s\n','m',dimstr(m));
  fprintf('  %20s : %s\n','omega',dimstr(omega));
  fprintf('  %20s : %s\n','IMAGE MODEL',imgModel);
  fprintf('  %20s : %s\n','DISTANCE',distance);
  fprintf('  %20s : %s\n','TRAFO',trafo);
  fprintf('  %20s : %s\n','#timeSteps',num2str(N));
  fprintf('  %20s : %s\n','nt',num2str(nt));
  fprintf('  %20s : %s\n','mV',dimstr(mV));
  fprintf('  %20s : %s\n','omegaV',dimstr(omegaV));
  fprintf('  %20s : %s\n','length(vc)',num2str(length(vc)));
  Jc = vc; % return starting guess
  return;
end;
tspan = [1 0];
dim   = numel(omega)/2;
nt    = round(numel(vc)/(dim*prod(m)))-1;

% do the work ------------------------------------------------------------
matrixFree   = regularizer('get','matrixFree');
doDerivative = (nargout>2);            % flag for necessity of derivatives

% compute transformation, distance, and regularization and combine these
if nt<1
    [yc,dy,pTrafo] = getTrafoFromVelocityRK4(vc,xc,'omega',omegaV,'m',mV,'N',N,'tspan',tspan,'doDerivative',doDerivative);
else
    [yc,dy,pTrafo] = getTrafoFromInstationaryVelocityRK4(vc,xc,omegaV,[mV,nt],'N',N,'tspan',tspan,'doDerivative',doDerivative);
end
[Tc,dT] = imgModel(T,omega,center(yc,m),'doDerivative',doDerivative);

% compute distance
[Dc,~,dD,dres,d2psi] = distance(Tc,Rc,omega,m,'doDerivative',doDerivative);

% compute regularizer
[Sc,dS,d2S] = regularizer(vc-vRef,omegaV,mV,'doDerivative',doDerivative,'tspan',tspan);

Jc = Dc + Sc;

% collect variables for plots
para = struct('Tc',Tc,'Rc',Rc,'omega',omega,'m',m,'yc',yc,'Jc',Jc,'Sc',Sc,'Dc',Dc,...
    'N',pTrafo.N);

if ~doDerivative, return; end;
dD = dD*dT*dy;
dJ = dD + dS;
if nargout<4, return; end;

% approximation to Hessian
dres = dres*dT*dy;

% multiply outer and inner derivatives, note: dy might be sparse
if not(matrixFree),
  H  = dres'*d2psi*dres + d2S;
else
    % approximation to d2D in matrix free mode
    % d2D   = dr'*d2psi*dr
    % P and P' are operators matrix free
    H.omega     = omegaV;
    H.m         = mV;
    H.nt        = nt;
    H.tspan     = tspan;
    H.d2D.how   = '*dr''*d2psi*dr';
    H.d2D.P     = @(x) x;
    H.d2D.dr    = dres;
    H.d2D.d2psi = d2psi;

    H.d2S = d2S;

end;

function runMinimalExample
setup2DGaussianData;
lvl = 5;
regularizer('reset','regularizer','mbElastic','alpha',1)
v0 = getVelocityStartingGuess(omega,m);
xc = getCellCenteredGrid(omega,ML{lvl}.m);
T  = reshape(imgModel(ML{lvl}.T,omega,center(xc,ML{lvl}.m)),ML{lvl}.m);
Rc = imgModel(ML{lvl}.R,omega,center(xc,ML{lvl}.m));

fctn = @(vc) LDDMMobjFctn(T,Rc,omega,ML{lvl}.m,v0,xc,omega,m,10,vc);
checkDerivative(fctn,v0)


function [GF2] = GreenFunction2(r0,r,f0)

parameter; % Define parameters
k0    = 2*pi*f0/c; % fixed wavenumber

GF2   = zeros(1,N+1); % Initialisation of array vector
mode0 = zeros(1,N+1);

% Construct a line from r0 to r
diff_r_r0 = r - r0;    %distance between r and r0
rz0 = [r(1);r(2);0];   %Projection onto x-y plane
diff_rz0_r = rz0 - r0; %distance between r in z-axis and r0

% Calculate the angles between the lines
angle1 = acos((diff_r_r0'*diff_rz0_r)/(norm(diff_r_r0)*norm(diff_rz0_r)));
angle2 = acos((diff_rz0_r'*rz0)/(norm(rz0)*norm(diff_rz0_r)));

Lvec = norm(diff_r_r0);  %length of line
Nvec = (0:1:N);
xpos = Nvec.*Lvec/N*cos(angle1)*sin(angle2) + r0(1);
ypos = Nvec.*Lvec/N*cos(angle1)*cos(angle2) + r0(2);
zpos = Nvec.*Lvec/N*sin(angle1)             + r0(3);

for d = 1:N+1

%%% Calculate the AXIAL MODE %%%
    %lx
    	N = fix(2*2*lx*f0/c); %number of axial modes below frequency f0
        nx = 1:N;
        rx = xpos(d);
    	[modeshape,k_term] = AxialMode(nx,lx,rx,r0(1),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));
    %ly
        N = fix(2*2*ly*f0/c); %number of axial modes below frequency f0
        ny = 1:N;
        ry = ypos(d);
    	[modeshape,k_term] = AxialMode(ny,ly,ry,r0(2),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));
    %lz
        N = fix(2*2*lz*f0/c); %number of axial modes below frequency f0
        nz = 1:N;
        rz = zpos(d);
    	[modeshape,k_term] = AxialMode(nz,lz,rz,r0(3),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));

%%% Calculate the TANGENTIAL MODE %%%
    %lx,ly
    	N = fix(2*pi*lx*ly*f0^2/c^2); %number of tangential modes below frequency f0
    	nx = 1:N; ny = nx;
        rx = xpos(d); ry = ypos(d);
    	[modeshape,k_term] = TangentialMode(nx,ny,lx,ly,rx,ry,r0(1),r0(2),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));
    %ly,lz
		N = fix(2*pi*ly*lz*f0^2/c^2); %number of tangential modes below frequency f0
        ny = 1:N; nz = ny;
        ry = ypos(d); rz = zpos(d);
    	[modeshape,k_term] = TangentialMode(ny,nz,ly,lz,ry,rz,r0(2),r0(3),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));
    %lx,lz
    	N = fix(2*pi*lx*lz*f0^2/c^2); %number of tangential modes below frequency f0
        nx = 1:N; nz = nx;
        rx = xpos(d); rz = zpos(d);
    	[modeshape,k_term] = TangentialMode(nx,nz,lx,lz,rx,rz,r0(1),r0(3),k0);
    	GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));

%%% Calculate the OBLIQUE MODE %%%
    %lx,ly,lz
    	N = fix(2*4*pi*lx*ly*lz*f0^3/(3*c^3)); %number of oblique modes below frequency f0
        ny = 1:N; nz = ny;
        rx = xpos(d); ry = ypos(d); rz = zpos(d);
        for nx = 1:N,
            [modeshape,k_term] = ObliqueMode(nx,ny,nz,lx,ly,lz,rx,ry,rz,r0(1),r0(2),r0(3),k0);
            GF2(d) = GF2(d) + (-1/V)*sum(sum(modeshape./k_term));
        end;

%%% Calculate the (0,0,0) mode %%%
    mode0 = -8/(V*k0.^2);

%%% Calculate the Green function plus the (0,0,0) mode
    GF2(d) = GF2(d) + mode0;
end;


function [a,T] = compute_tf_model(xyu_trimmed, p_drone, p_physics)
% COMPUTE_TF_MODEL - Function computing the transfer function model
%
% Syntax: out = airdata(x)
%
% Inputs:
%   x_trim - desired trimmed state
%   u_trim - desired trimmed input delta
%   y_trim - desired trimmed air data
%   P      - parameters structure
%
% Outputs:
%   T_phi_da        - transfer function (tf) from delta_aileron to roll
%   T_chi_phi       - tf from chi to roll
%   T_theta_de      - tf from delta_elevator to pitch
%   T_h_theta       - tf from pitch to altitude
%   T_h_Va          - tf from Va to altitude
%   T_Va_dt         - tf from delta_thrust to Va
%   T_Va_theta      - tf from pitch to Va
%   T_vy_dr         - tf from delta_rudder to vy

x_trim = xyu_trimmed(1:12);
y_trim = xyu_trimmed(13:15);
u_trim = xyu_trimmed(16:19);

% Relabel inputs
% Position in NED
% pn          = x_trim(1);
% pe          = x_trim(2);
% pd          = x_trim(3); % -altitude

% UAV velocity wrt inertial frame in body frame
% vx          = x_trim(4);
% vy          = x_trim(5);
% vz          = x_trim(6);
% v_xyz       = x_trim(4:6);

% Euler angles
% phi         = x_trim(7);
theta       = x_trim(8);
% psi         = x_trim(9);

% Rotation rates
% p           = x_trim(10);
% q           = x_trim(11);
% r           = x_trim(12);

% Actuators
de = u_trim(1); % delta elevator
% da = u_trim(2); % delta aileron
% dr = u_trim(3); % delta rudder
dt = u_trim(4); % delta thrust

% Wind
Va =    y_trim(1);
alpha = y_trim(2);
% beta =  y_trim(3);

% Compute parametrs
p_dyn = 0.5*p_physics.rho*Va^2;
kb = p_drone.kb(Va);
if isinf(kb)
    kb =0;
end
kc = p_drone.kc(Va);
if isinf(kc)
    kc =0;
end

a.phi1 = - p_dyn*p_drone.S_wing*p_drone.b*p_drone.Cp_p*kb;
a.phi2 =   p_dyn*p_drone.S_wing*p_drone.b*p_drone.Cp_da;

a.beta1 = - 0.5 * p_physics.rho * Va * p_drone.S_wing / p_drone.mass * p_drone.CY_beta;
a.beta2 =   0.5 * p_physics.rho * Va * p_drone.S_wing / p_drone.mass * p_drone.CY_dr;

a.theta1 = - p_dyn * p_drone.c * p_drone.S_wing / p_drone.Jy * p_drone.Cm_q * kc;
a.theta2 = - p_dyn * p_drone.c * p_drone.S_wing / p_drone.Jy * p_drone.Cm_alpha;
a.theta3 =   p_dyn * p_drone.c * p_drone.S_wing / p_drone.Jy * p_drone.Cm_de;

a.va1 = p_physics.rho * Va * p_drone.S_wing / p_drone.mass * ( p_drone.CD0 + p_drone.CD_alpha*alpha + p_drone.CD_de*de ) + ...
       p_physics.rho * p_drone.S_prop / p_drone.mass * p_drone.C_prop * Va;
a.va2 = p_physics.rho * p_drone.S_prop / p_drone.mass * p_drone.C_prop * p_drone.k_motor^2 * dt;
a.va3 = p_physics.gravity * cos(theta - alpha);

% Define transfer functions
T.phi_da   = tf([a.phi2],[1,a.phi1,0]);
T.chi_phi       = tf([p_physics.gravity/Va],[1,0]);
T.theta_de = tf(a.theta3,[1,a.theta1,a.theta2]);
T.h_theta       = tf([Va],[1,0]);
T.h_Va          = tf([theta],[1,0]);
T.Va_dt    = tf([a.va2],[1,a.va1]);
T.Va_theta      = tf([-a.va3],[1,a.va1]);
T.vy_dr     = tf([Va*a.beta2],[1,a.beta1]);


function [x, infos] = sparse_nmf(V, rank, in_options)
% Sparse nonnegative matrix factorization (sparseNMF)
%
% The problem of interest is defined as
%
%       min D(V||W*H) + lambda * sum(H(:)),
%       where
%       {V, W, H} > 0.
%
%       L1-based sparsity constraint on H.
%       Normalizes W column-wise.
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       V           : (m x n) non-negative matrix to factorize
%       rank        : rank
%       in_options
%
%
% Output:
%       x           : non-negative matrix solution, i.e., x.W: (m x rank), x.H: (rank x n)
%       infos       : log information
%           epoch   : iteration nuber
%           cost    : objective function value
%           optgap  : optimality gap
%           time    : elapsed time
%           grad_calc_count : number of sampled data elements (gradient calculations)
%
% Reference:
%
%
% This file is part of NMFLibrary.
%
% Created by Patrik Hoyer, 2006 (and modified by Silja Polvi-Huttunen,
% University of Helsinki, Finland, 2014)
%
% Modified by H.Kasai on Jul. 23, 2018
%
% Change log:
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 14, 2022 (Hiroyuki Kasai): Fixed algorithm.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options.lambda = 0;   % regularizer for sparsity
    local_options.cost  = 'euc'; % 'euc' or 'kl-div'

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = 'sparseNMF';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);
    % store additionally different cost
    reg_val = options.lambda*sum(sum(H));
    f_val_total = f_val + reg_val;
    infos.cost_reg = reg_val;
    infos.cost_total = f_val_total;

    if options.verbose > 1
        fprintf('sparseNMF: Epoch = 0000, cost = %.16e, optgap = %.4e\n', f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % update H with MU
        %H = (H.*(W'*(V./(W*H))))/(1+alpha);
        VC = V./(W*H + 1e-9);
        VC(V==0 & W*H==0) = 1+1e-9;
        H = (H.*(W'*VC))/(1+options.lambda);

        % update W by Lee and Seung's divergence step
        %W = W.*((V./(W*H))*H')./(ones(vdim,1)*sum(H'));
        VC = V./(W*H + 1e-9);
        VC(V==0 & W*H==0) = 1+1e-9;
        W = W.*(VC*H')./(ones(m,1)*sum(H,2)');

        % Liu, Zheng, and Lu add this normalization step
        W = W./(ones(m,1)*sum(W));

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        epoch = epoch + 1;

        % store info
        [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);
        % store additionally different cost
        reg_val = options.lambda*sum(sum(H));
        f_val_total = f_val + reg_val;
        infos.cost_reg = [infos.cost_reg reg_val];
        infos.cost_total = [infos.cost_total f_val_total];

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end


function x = haar ( n, x )

%*****************************************************************************80
%
%% HAAR performs a Haar transform.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2011
%
%  Author:
%
%    Ken Beauchamp
%
%  Reference:
%
%    Ken Beauchamp,
%    Walsh functions and their applications,
%    Academic Press, 1975,
%    ISBN: 0-12-084050-2,
%    LC: QA404.5.B33.
%
%  Parameters:
%
%    Input, integer N, the number of items in X.
%    N must be a power of 2.
%
%    Input, real X(N), the data to be transformed.
%
%    Output, real X(N), the transformed data.
%
  k = i4_log_2 ( n );

  for i = 1 : k

    l = k + 1 - i;
    l2 = 2^( l - 1 );

    y(1:2*l2) = x(1:2*l2);

    for j = 1 : l2
       l3 = l2 + j;
       jj = 2 * j - 1;
       x(j) = y(jj) + y(jj+1);
       x(l3) = y(jj) - y(jj+1);
    end

  end

  return
end


% capillary pressure curve for drainage
% Written by Ali A. Eftekhari
function res=dpc_drain(sw, pce, swc, labda)
pc0=1.0e7;
res=zeros(size(sw));
for i=1:numel(sw)
  sw0=swc(i)+(1-labda*log(pc0/pce(i))+sqrt((-1+labda*log(pc0/pce(i)))^2+...
      4*swc(i)/(1-swc(i))))/2*(1-swc(i));
  if sw(i)>sw0
    res(i)=-1.0/((1-swc(i))*labda)*pce(i)*((sw(i)-swc(i))/(1-swc(i)))^(-1.0/labda-1);
  elseif 0.0<=sw(i) && sw(i)<=sw0
    res(i)=-1.0/((1-swc(i))*labda)*pce(i)*((sw0-swc(i))/(1-swc(i)))^(-1.0/labda-1);
  else
    res(i)=0.0;
  end
end


function [SurfCov,Dis,CylVol,dis] = surface_coverage(P,Axis,Point,nl,ns,Dmin,Dmax)

% ---------------------------------------------------------------------
% SURFACE_COVERAGE.M   Computes point surface coverage measure
%
% Version       1.1.0
% Last update   7 Oct 2021
%
% Copyright (C) 2017-2021 Pasi Raumonen
% ---------------------------------------------------------------------
% Inputs:
% Axis      Axis direction (1 x 3)
% Point     Starting point of the cylinder (1 x 3)
% nl        Number of layers in the axis direction used for to partition
%               the cylinder surface into layer/sectors
% ns        Number of sectors used to partition the cylinder surface into
%               layer/sectors
% Dmin      (Optional) Minimum point distance from the axis to be included
%               into SurfCov calculations
% Dmax      (Optional) Maximum point distance from the axis to be included
%               into SurfCov calculations
%
% Output:
% SurfCov   Number between 0 and 1 descring how big portion of the cylinder
%               surface is covered with points
% Dis       (Optional) Mean distances of the distances of the layer/sectors
% CylVol    (Optional) Volume of the cylinder estimated by the mean
%               distances of the layer/sectors as cylindrical sections
% dis       (Optional) Same as "Dis" but empty cells are interpolated
% ---------------------------------------------------------------------
% Computes surface coverage (number between 0 and 1) of points on cylinder
% surface defined by "Axis" and "Point".

% Changes from version 1.0.0 to 1.1.0, 7 Oct 2021:
% 1) Added two possible inputs, minimum and maximum distance,
%    Dmin and Dmax, which can be used to filter out points for the surface
%    coverage calculations
% 2) Computes the SurfCov estimate with four baseline directions used in
%    the sector determination and selects the largest value
% 3) Smalle changes to speed up computations

%% Compute the distances and heights of the points
[d,V,h] = distances_to_line(P,Axis,Point);
h = h-min(h);
Len = max(h);

%% (Optional) Filter out points based on the distance to the axis
if nargin >= 6
  Keep = d > Dmin;
  if nargin == 7
    Keep = Keep & d < Dmax;
  end
  V = V(Keep,:);
  h = h(Keep);
end

%% Compute SurfCov
% from 4 different baseline directions to determine the angles and select
% the maximum value
V0 = V;
[U,W] = orthonormal_vectors(Axis); % First planar axes
R = rotation_matrix(Axis,2*pi/ns/4); % Rotation matrix to rotate the axes
SurfCov = zeros(1,4);
for i = 1:4
  %% Rotate the axes
  if i > 1
    U = R*U;
    W = R*W;
  end

  %% Compute the angles (sectors) of the points
  V = V0*[U W];
  ang = atan2(V(:,2),V(:,1))+pi;

  %% Compute lexicographic order (sector,layer) of every point
  Layer = ceil(h/Len*nl);
  Layer(Layer <= 0) = 1;
  Layer(Layer > nl) = nl;
  Sector = ceil(ang/2/pi*ns);
  Sector(Sector <= 0) = 1;
  LexOrd = [Layer Sector-1]*[1 nl]';

  %% Compute SurfCov
  Cov = zeros(nl,ns);
  Cov(LexOrd) = 1;
  SurfCov(i) = nnz(Cov)/nl/ns;
end
SurfCov = max(SurfCov);

%% Compute volume estimate
if nargout > 1
  % Sort according to increasing lexicographic order
  [LexOrd,SortOrd] = sort(LexOrd);
  d = d(SortOrd);

  % Compute mean distance of the sector-layer intersections
  Dis = zeros(nl,ns); % mean distances
  np = length(LexOrd);     % number of points
  p = 1;
  while p <= np
    t = 1;
    while (p+t <= np) && (LexOrd(p) == LexOrd(p+t))
      t = t+1;
    end
    Dis(LexOrd(p)) = average(d(p:p+t-1));
    p = p+t;
  end

  if nargout > 2
    % Interpolate missing distances
    D = Dis;
    dis = Dis;
    Dinv = D((nl:-1:1)',:);
    D = [Dinv Dinv Dinv; D D D; Dinv Dinv Dinv];
    Zero = Dis == 0;
    RadMean = average(Dis(Dis > 0));
    for i = 1:nl
      for j = 1:ns
        if Zero(i,j)
          if nnz(D(i+nl-1:i+nl+1,j+ns-1:j+ns+1)) > 1
            d = D(i+nl-1:i+nl+1,j+ns-1:j+ns+1);
            dis(i,j) = average(d(d > 0));
          elseif nnz(D(i+nl-2:i+nl+2,j+ns-2:j+ns+2)) > 1
            d = D(i+nl-2:i+nl+2,j+ns-2:j+ns+2);
            dis(i,j) = average(d(d > 0));
          elseif nnz(D(i+nl-3:i+nl+3,j+ns-3:j+ns+3)) > 1
            d = D(i+nl-3:i+nl+3,j+ns-3:j+ns+3);
            dis(i,j) = average(d(d > 0));
          else
            dis(i,j) = RadMean;
          end
        end
      end
    end
    % Compute the volume estimate
    r = dis(:);
    CylVol = 1000*pi*sum(r.^2)/ns*Len/nl;
  end
end


%IISUM Sum of integral image
%
% S = IISUM(II, U1, V1, U2, V2) is the sum of pixels in the rectangular image
% region defined by its top-left (U1,V1) and bottom-right (U2,V2).  II is
% a precomputed integral image.
%
% See also INTGIMAGE.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

function s = iisum(ii, c1, r1, c2, r2)

    r1 = r1 - 1;
    if r1 < 1
        sA = 0;
        sB = 0;
    else
        sB = ii(r1,c2);
    end
    c1 = c1 - 1;
    if c1 < 1
        sA = 0;
        sC = 0;
    else
        sC = ii(r2,c1);
    end
    if (r1 >= 1) && (c1 >= 1)
        sA = ii(r1,c1);
    end

    s = ii(r2,c2) + sA -sB - sC;