clear;
t=0:0.01:1;% wektor z wartosciami od 0 do 1 rosnace o 0.01
X=0.5*sin(2*pi*15*t)+2*sin(2*pi*t)+sin(2*pi*7*t);  %tworzymy sobie sygnal wyjsciowy - suma trzech sinusoid

windowSize = 10;
Num = (1/windowSize)*ones(1,windowSize);
load FFR;
Pp1=filter(Num,1,X) ;           %filtr dolnoprzepustowy Fc:10 Hz
load FFR;
Pp2=filter(Num,1,X);            %filtrdolnoprzepustowy Fc: 5 Hz


Lmax=15;                        %poczatkowo przyjmowany maksymalny rzad macierzy P
N=size(t,2);                    %N - liczba wykorzystanych probek sygnalu


Ptp=returnPtP(N,Lmax,Pp1,Pp2);    %Ta funkcja zwraca macierz, ktora powstaje z pomnozenia P transpon. z P (czyli PtP)


[e_vec,e]=eig(Ptp);               %tu szukamy wartosci wlasnych macierzy PtP
                                %(zmienna e) i jej wektorów wlasnych (zmienna e_vec)
                                %smienne e i e_vec są macierzami

eMax = max(max(e));             %szukamy najwiekszej wartosci wlasnej macierzy PtP
                                %pierwsze max(e) daje nam wektor z
                                %najwiekszymi wartościami w kolumnach
                                %macierzy e, drugie max(max(e)) daje nam
                                %wartość maxymalną z wektora max(e)


%%%%%%%ponizsza petla wyznacza ile jest wartosci wlasnych macierzy PtP,
%%%%%%%ktore sa wieksze lub równe 5% najwiekszej wartosci wlasnej
k=0;
for i=1:size(e)

    if e(i,i)>=0.05*eMax
        k=k+1;
    end

end

odd_or_not = mod(k,2);

%%%%%%%%%%%%%%%Okreslamy nowt rzad macierzy

if odd_or_not==1
k=k+1;          %jesli petla zliczy nieparzysta liczbe to zaokraglamy w gore
L=0.5*k;
else
L=0.5*k;        %polowa wartosci wlasnych zliczonych przez powyzsza petle to nowy rzad filtru
end

%%%%%%%%%%%%%%%Ponownie liczymy macierz PtP juz majac wlasciwy oszacowany rzad (moze on wyjsc duzo nizszy niz rzeczywisty rzad filtrow)

Ph=returnPtP(N,L,Pp1,Pp2);

[V,E]=eig(Ph);  % zwraca diagonalna macierz wartosci wlasnych E i macierz V gdzie kolumny macierzy V sa odpowiednimi wektorami wlasnymi, macierzy PtP

E_dgl = diag(E);                            %otrzymujemy wektor zawierajacy wartosci z przekatnej macierzy diagonalnej E
Emin=min(E_dgl);                            %szukamy najmniejszej wartosci wlasnej macierzy PtP
[Emin_row,Emin_col]=find(E==Emin,1);        %sprawdzamy jej indeks
h=V(:,Emin_col);                            %z wektora V wybieramy odpowiadajacy wektor wlasny
                                            %to bedzie nasz wektor h

m=size(h);
h1=h(1:(m/2));                              %dzielimy go sobie po polowie na 2 tak ze pol wektora to h1 a drugie pol to h2

h2=h(((m/2)+1):m);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%transponujemy Pp1 i Pp2
Pp1_vec=Pp1';
Pp2_vec=Pp2';
Pp=[Pp1_vec;Pp2_vec];

%%%%%%%%%%%%%%%%%%%%%%%%%%%tutaj nizej z wektorow h tworzymy macierze toeplitza
%%%%%%%%%%%%%%%%%%%%%%%%%%%oznaczone duzym H (wpiszcie w wikipedie toeplitz matrix to zobaczycie rozmieszczenie elementow)
c1=zeros(N,1);
c2=zeros(N,1);
r1=zeros(1,N+L-1);
r2=zeros(1,N+L-1);

for i=1:(m/2)
    c1(i)=h1(i);                     %tu tworzymy pierwsze kolumny H1 i H2
    c2(1)=h2(i);
end


r1(1)=c1(1);                        %tu tworzymy pierwsze wiersze H1 i H2
r2(1)=c2(1);
H1=toeplitz(c1,r1);                 %tworzenie macierzy H1 i H2
H2=toeplitz(c2,r2);

H=[H1;H2];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Metoda najmniejszych kwadratow

Nom=H'*Pp;                           %tworzymy licznik
Denom=(H'*H);                        %tworzymy mianownik
Pa=mldivide(Nom,Denom);              %dzielimy przez siebie
Pa=Pa(1:N);                          %czasem trzeba uciąć sygnal do interesującej nas długości stąd ten zapis
Pa_s=Pa*(mean(Pp1)/mean(Pa));        %skalowanie otrzymanego sygnału, żeby miał porownywalną wartość średnią do pomierzonych (wzór A6)

plot(t/2,Pa_s,'r');


 subplot(2,2,1);plot(t,Pp1);title('FIR1');
 subplot(2,2,2);plot(t,Pp2);title('FIR2');
 subplot(2,2,3);plot(t,X); title('original');
 subplot(2,2,4);plot(t,Pa_s,'r');title('reconstructed');