load('data_ASTANK2.mat');
y1=Did.y(:,1);
y2=Did.y(:,2);
u1=Did.u(:,1);
u2=Did.u(:,2);

flip_u1=flip(u1);
flip_u2=flip(u2);
flip_y1=flip(y1);
flip_y2=flip(y2);

D1 = iddata(Did.y(:,1), [Did.u(:,1) Did.u(:,2)]);
D2 = iddata(Did.y(:,2), [Did.u(:,1) Did.u(:,2)]);

y1_norm = norm(detrend(Did.y(:,1), 'constant'));
y2_norm = norm(detrend(Did.y(:,2), 'constant'));

index1 = randi([1 30],20,3);
index2 = randi([1 30],20,3);

index1ARMA = randi([50 90],10,2);
index2ARMA = randi([50 90],10,2);

M1 = cell(20, 1);
M2 = cell(20, 1);

M1ARMA = cell(20, 1);
M2ARMA = cell(20, 1);

v1=cell(20,1);
v2=cell(20,1);

e1=cell(10,1);
e2=cell(10,1);

SNR1=zeros(20,1);
SNR2=zeros(20,1);

SNR1ARMA=zeros(10,1);
SNR2ARMA=zeros(10,1);

ys1=cell(20,1);
ys2=cell(20,1);

ys1ARMA=cell(20,1);
ys2ARMA=cell(20,1);




for i = 1 : 20
    disp(i)
   try
       M1{i} = armax(D1, [index1(i, 1) [index1(i, 2) index1(i, 3)] 0 [1 1]]);
   catch
       M1{i}=[]
   end

   try
      M2{i} = armax(D2, [index2(i, 1) [index2(i, 2) index2(i, 3)] 0 [1 1]]);
   catch
       M2{i}=[]
   end

   max1 = max(index1(i,:));
   max2 = max(index2(i,:));
   ys1{i} = y1(1:max1);
   ys2{i} = y2(1:max2);

   a_m1=M1{i}.a(2:end);
   a_m2=M2{i}.a(2:end);

   b1_m1=M1{i}.b{1}(2:end);
   b2_m1=M1{i}.b{2}(2:end);

   b1_m2=M2{i}.b{1}(2:end);
   b2_m2=M2{i}.b{2}(2:end);

   if(isempty(M1{i}))
       ys1{i}=[];
   else
        ys=ys1{i};
        for j = (max1 +1) : size(y1, 1)
         % v1{i}=y1-[y1_ARX{i};zeros(size(y1,1)-size(y1_ARX{i},1),1)];
         %nu de la 1
        % ys=b1_m1*flip_u1(1:size(b1_m1,2))+b2_m1*flip_u2(1:size(b2_m1,2))-(a_m1*flip_y1(1:size(a_m1,2)))
         %yst=b1_m1*flip_u1((j-size(b1_m1,2)):(j-1))+b2_m1*flip_u2((j-size(b2_m1,2)):(j-1))-a_m1*ys((j-1):-1:(j-size(a_m1,2)))
             yst=b1_m1*flip(u1((j-size(b1_m1,2)):(j-1)))+b2_m1*flip(u2((j-size(b2_m1,2)):(j-1)))-a_m1*ys((j-1):-1:(j-size(a_m1,2)));
         %yst=b1_m1*u1(j-1:-1:j-length(b1_m1))+b2_m1*u2(j-1:-1:j-length(b2_m1))-a_m1*ys((j-1):-1:(j-size(a_m1,2)));
             ys=[ys;yst];
        end
        ys1{i}=ys;
    end
    v1{i}=y1-ys1{i};

% figure
% plot(y1);
% hold on
% plot (ys);
%pause


    if(isempty(M2{i}))
        ys2{i}=[];
    else
        ys=ys2{i};
        for j = (max2 +1) : size(y1, 1)
            yst=b1_m2*flip(u1((j-size(b1_m2,2)):(j-1)))+b2_m2*flip(u2((j-size(b2_m2,2)):(j-1)))-a_m2*ys((j-1):-1:(j-size(a_m2,2)));
            ys=[ys;yst];
        end
        ys2{i}=ys;
    end
    v2{i}=y2-ys2{i};

end




for i=1:20


    if isempty(ys1{i})
        SNR1(i)=0;
    else
        SNR1(i)=20*log(y1_norm/norm(v1{i}));

    end
    if (SNR1(i)<0 || isnan(SNR1(i)))
         SNR1(i)=0;
    end

    if isempty(ys2{i})
        SNR2(i)=0;
    else
        SNR2(i)=20*log(y2_norm/norm(v2{i}));

    end
    if (SNR2(i)<0 || isnan(SNR2(i)))
         SNR2(i)=0;
    end


    [MSnr1,poz1ARX]=max(SNR1);
    [MSnr2,poz2ARX]=max(SNR2);

    D1ARMA=iddata(v1{poz1ARX});
    D2ARMA=iddata(v2{poz2ARX});

    vStat1_norm=norm(detrend(v1{poz1ARX}, 'constant'));
    vStat2_norm=norm(detrend(v2{poz2ARX}, 'constant'));
end





for i = 1 : 10
    disp(i)
   try
       M1ARMA{i} = armax(D1ARMA, [index1ARMA(i, 1) index1ARMA(i, 2)]);
   catch
       M1ARMA{i}=[];
   end

   try
        M2ARMA{i} = armax(D2ARMA, [index2ARMA(i, 1) index2ARMA(i, 2)]);
   catch
        M2ARMA{i}=[];
   end

   if isempty(M1ARMA{i})
        ys1ARMA{i}=[];
   else
        ys1ARMA{i}=predict(M1ARMA{i}, D1ARMA);
        e1{i}=v1{poz1ARX}-ys1ARMA{i}.y;
   end

   if isempty(M2ARMA{i})
        ys2ARMA{i}=[];
   else
        ys2ARMA{i}=predict(M2ARMA{i}, D2ARMA);
        e2{i}=v2{poz2ARX}-ys2ARMA{i}.y;
   end


end





for i=1:10


    if isempty(ys1ARMA{i})
        SNR1ARMA(i)=0;
    else
        SNR1ARMA(i)=20*log(vStat1_norm/norm(e1{i}));

    end
    if (SNR1ARMA(i)<0 || isnan(SNR1ARMA(i)))
         SNR1ARMA(i)=0;
    end

    if isempty(ys2ARMA{i})
        SNR2ARMA(i)=0;
    else
        SNR2ARMA(i)=20*log(vStat2_norm/norm(e2{i}));

    end
    if ( isnan(SNR2ARMA(i)))
         SNR2ARMA(i)=0;
    end



    [MSnr1ARMA,poz1ARMA]=max(SNR1ARMA);
    [MSnr2ARMA,poz2ARMA]=max(SNR2ARMA);

    Yfinal1=ys1{poz1ARX}+ys1ARMA{poz1ARMA}.y;
    Yfinal2=ys2{poz2ARX}+ys2ARMA{poz2ARMA}.y;
end




plot(ys1{poz1ARX})
hold on
plot(y1)
legend('ARX1','y1');
title('Y1ARX vs y1');
text(500,10.2,"SNR="+num2str(SNR1(poz1ARX)))
figure

plot(ys2{poz2ARX})
hold on
plot(y2)
title('Y2ARX  vs y2');
legend('ARX2','y2');
text(500,10,"SNR="+num2str(SNR2(poz2ARX)))
figure


plot(Yfinal1)
hold on
plot(y1)
legend('Yfinal1','y1');
title('Y1ARX+V1ARMA vs y1');
figure


plot(Yfinal2)
hold on
plot(y2)
legend('Yfinal2','y2');
title('Y2ARX+V2ARMA vs y2');
figure


plot(e1{poz1ARMA})
title('eps1');
text(500,-0.2,"λe1^2="+num2str(norm(e1{poz1ARMA})))
figure

plot(e2{poz2ARMA})
title('eps2');
text(500,-0.2,"λe2^2="+num2str(norm(e2{poz2ARMA})))