%% PŘIZPŮSOBENÝ FILTR UCHOPENÍ Z BOČNICE:
% Skript pro tvorbu přizpůsobeného filtru, který detekuje uchopení z bočnice pro zapnutí
% ovladače
% Ve skriptu jsou vytvořeny impulzní charakteristiky a následně jsou
% provedeny korelace se signály s úseky uchopení a tyto vykresleny do grafů

%% Nahrání již uložených impulzních charakteristik a signálů:
clear all, close all, clc

load('AXZ.mat')
load('impChars.mat')

%{
%% Nahrání a výběr požadovaných signálů:
signaly = load('mereniNemocnice.mat');
nazvySignalu = fieldnames(signaly);

prvni = signaly.A4b1;
druhy = signaly.A4b2;
treti = signaly.A4c1;
ctvrty = signaly.A4c2;
paty = signaly.A4d1;
sesty = signaly.A4d2;


%% Část úpravy raw dat:
% Úprava raw dat z měřícího přípravku:
[signal1,x1,y1,z1,cas1] = upravSignal(prvni,0);
[signal2,x2,y2,z2,cas2] = upravSignal(druhy,0); 
[signal3,x3,y3,z3,cas3] = upravSignal(treti,0);
[signal4,x4,y4,z4,cas4] = upravSignal(ctvrty,0);
[signal5,x5,y5,z5,cas5] = upravSignal(paty,0);
[signal6,x6,y6,z6,cas6] = upravSignal(sesty,0);
XZ = {x1,x2,x3,x4,x5,x6;z1,z2,z3,z4,z5,z6};

% Výpočet výsledného zrychlení (požadovaný vstup fce 'filtrace'):
a1 = vyslednyVektor(x1,y1,z1,cas1,0);
a2 = vyslednyVektor(x2,y2,z2,cas2,0);
a3 = vyslednyVektor(x3,y3,z3,cas3,0);
a4 = vyslednyVektor(x4,y4,z4,cas4,0);
a5 = vyslednyVektor(x5,y5,z5,cas5,0);
a6 = vyslednyVektor(x6,y6,z6,cas6,0);
A = {a1,a2,a3,a4,a5,a6};

%% Filtrace:
% Parametry filtru:
fVz = 71.07;
n = 20;
fMDP = 20;
fMHP = 3;
% [aFilt,xFilt,yFilt,zFilt] = filtraceHP(celkZrych,x,y,z,cas,fVz,n,fM,logGraf)
[aDP1,xDP1,yDP1,zDP1] = filtraceDP(a1,x1,y1,z1,cas1,fVz,n,fMDP,0);
[aDP2,xDP2,yDP2,zDP2] = filtraceDP(a2,x2,y2,z2,cas2,fVz,n,fMDP,0);
[aDP3,xDP3,yDP3,zDP3] = filtraceDP(a3,x3,y3,z3,cas3,fVz,n,fMDP,0);
[aDP4,xDP4,yDP4,zDP4] = filtraceDP(a4,x4,y4,z4,cas4,fVz,n,fMDP,0);
[aDP5,xDP5,yDP5,zDP5] = filtraceDP(a5,x5,y5,z5,cas5,fVz,n,fMDP,0);
[aDP6,xDP6,yDP6,zDP6] = filtraceDP(a6,x6,y6,z6,cas6,fVz,n,fMDP,0);

[aFilt1,xFilt1,yFilt1,zFilt1] = filtraceHP(aDP1,xDP1,yDP1,zDP1,cas1,fVz,n,fMHP,0);
[aFilt2,xFilt2,yFilt2,zFilt2] = filtraceHP(aDP2,xDP2,yDP2,zDP2,cas2,fVz,n,fMHP,0);
[aFilt3,xFilt3,yFilt3,zFilt3] = filtraceHP(aDP3,xDP3,yDP3,zDP3,cas3,fVz,n,fMHP,0);
[aFilt4,xFilt4,yFilt4,zFilt4] = filtraceHP(aDP4,xDP4,yDP4,zDP4,cas4,fVz,n,fMHP,0);
[aFilt5,xFilt5,yFilt5,zFilt5] = filtraceHP(aDP5,xDP5,yDP5,zDP5,cas5,fVz,n,fMHP,0);
[aFilt6,xFilt6,yFilt6,zFilt6] = filtraceHP(aDP6,xDP6,yDP6,zDP6,cas6,fVz,n,fMHP,0);

XZFilt = {xFilt1,xFilt2,xFilt3,xFilt4,xFilt5,xFilt6;zFilt1,zFilt2,zFilt3,zFilt4,zFilt5,zFilt6};
AFilt = {aFilt1,aFilt2,aFilt3,aFilt4,aFilt5,aFilt6};

%% Část výběru a tvorby imp. charakteristik:
% Výběr ze signálů výsledného zrychlení - bez filtrace:
a1PF = fliplr(a1(268:376));
a2PF = fliplr(a2(198:335));
a3PF = fliplr(a3(285:502));
a4PF = fliplr(a4(271:383));
a5PF = fliplr(a5(201:310));
a6PF = fliplr(a6(355:457));
APF = {a1PF,a2PF,a3PF,a4PF,a5PF,a6PF};

% Výběr ze signálů výsledného zrychlení - filtrovaných:
aFilt1PF = fliplr(aFilt1((268:376)+n)); %/2));
aFilt2PF = fliplr(aFilt2((198:335)+n)); %/2));
aFilt3PF = fliplr(aFilt3((285:502)+n)); %/2));
aFilt4PF = fliplr(aFilt4((271:383)+n)); %/2));
aFilt5PF = fliplr(aFilt5((201:310)+n)); %/2));
aFilt6PF = fliplr(aFilt6((355:457)+n)); %/2));
AFiltPF = {aFilt1PF,aFilt2PF,aFilt3PF,aFilt4PF,aFilt5PF,aFilt6PF};

% Výběr ze signálů os X,Z - bez filtrace:
x1PF = fliplr(x1(262:384));
z1PF = fliplr(z1(262:384));
x2PF = fliplr(x2(198:312));
z2PF = fliplr(z2(198:312));
x3PF = fliplr(x3(266:384));
z3PF = fliplr(z3(266:384));
x4PF = fliplr(x4(273:388));
z4PF = fliplr(z4(273:388));
x5PF = fliplr(x5(194:306));
z5PF = fliplr(z5(194:306));
x6PF = fliplr(x6(349:445));
z6PF = fliplr(z6(349:445));
XZPF = {x1PF,x2PF,x3PF,x4PF,x5PF,x6PF;z1PF,z2PF,z3PF,z4PF,z5PF,z6PF};

% Výběr ze signálů os X,Z - filtrovaných:
xFilt1PF = fliplr(xFilt1((262:384)+n));  %/2));
zFilt1PF = fliplr(zFilt1((262:384)+n));  %/2));
xFilt2PF = fliplr(xFilt2((198:312)+n));  %/2));
zFilt2PF = fliplr(zFilt2((198:312)+n));  %/2));
xFilt3PF = fliplr(xFilt3((266:384)+n));  %/2));
zFilt3PF = fliplr(zFilt3((266:384)+n));  %/2));
xFilt4PF = fliplr(xFilt4((273:388)+n));  %/2));
zFilt4PF = fliplr(zFilt4((273:388)+n));  %/2));
xFilt5PF = fliplr(xFilt5((194:306)+n));  %/2));
zFilt5PF = fliplr(zFilt5((194:306)+n));  %/2));
xFilt6PF = fliplr(xFilt6((349:445)+n));  %/2));
zFilt6PF = fliplr(zFilt6((349:445)+n));  %/2));
XZFiltPF = {xFilt1PF,xFilt2PF,xFilt3PF,xFilt4PF,xFilt5PF,xFilt6PF;zFilt1PF,zFilt2PF,zFilt3PF,zFilt4PF,zFilt5PF,zFilt6PF};

cd 'C:\Users\ferdi\Documents\DOKUMENTY\Škola\VUT\6. semestr\ABCE\MatLab\BP\Přizpůsobený filtr Bočnice'
save('AXZ.mat','XZ','A','XZFilt','AFilt')
save('impChars.mat','APF','AFiltPF','XZPF','XZFiltPF')
cd 'C:\Users\ferdi\Documents\DOKUMENTY\Škola\VUT\6. semestr\ABCE\MatLab\BP'
%}

% {
%% Výpočet korelací a jejich vykreslení:
% Pro výsledné zrychlení - bez filtrace:
for i = 1:6
    impChar = APF{1,i};
    figure('Position',[0,0,1000,750])
    sgtitle(['Impulzní charakteristika ',num2str(i)])
    for j = 1:6
        korelace = conv(A{1,j},impChar);
        upravaKorelace = abs(korelace);
        pocetPrvku = length(A{1,j})+length(impChar)-1;
        rozsah = -(length(impChar)-1):(pocetPrvku-length(impChar));
        
        % Graf:
        subplot(6,1,j)
        plot(rozsah,upravaKorelace)
        title(['Signál výsledného zrychlení ',num2str(j)])
        xlim([min(rozsah)-10,max(rozsah)+10])
        ylim([-0.05*max(upravaKorelace),1.1*max(upravaKorelace)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
    end
    % Graf imp.char.:
    figure(100)
    subplot(6,1,i)
    plot(impChar)
    title(['Impulzní charakteristika ', num2str(i)])
    xlim([-100,length(impChar)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')

end
%}

% Pro výsledné zrychlení - filtrované:
for i = 1:6
    impChar = AFiltPF{1,i};
    figure('Position',[0,0,1000,750])
    sgtitle(['Impulzní charakteristika ',num2str(i)])
    for j = 1:6
        korelace = conv(AFilt{1,j},impChar);
        upravaKorelace = abs(korelace);
        pocetPrvku = length(AFilt{1,j})+length(impChar)-1;
        rozsah = -(length(impChar)-1):(pocetPrvku-length(impChar));
        
        % Graf:
        subplot(6,1,j)
        plot(rozsah,upravaKorelace)
        title(['Filtrovaný signál výsledného zrychlení ',num2str(j)])
        xlim([min(rozsah)-10,max(rozsah)+10])
        ylim([-0.05*max(upravaKorelace),1.1*max(upravaKorelace)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
    end
    % Graf imp.char.:
    figure(101)
    subplot(6,1,i)
    plot(impChar)
    title(['Impulzní charakteristika ', num2str(i)])
    xlim([-100,length(impChar)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')
end

% Pro signály X,Z - bez filtrace:
for i = 1:6
    impCharX = XZPF{1,i};
    impCharZ = XZPF{2,i};
    figure('Position',[0,0,1000,750])
    sgtitle(['Impulzní charakteristiky X,Z ',num2str(i)])
    for j = 1:6
        korelaceX = conv(XZ{1,j},impCharX);
        korelaceZ = conv(XZ{2,j},impCharZ);
        upravaKorelaceX = abs(korelaceX);
        upravaKorelaceZ = abs(korelaceZ);
        pocetPrvkuX = length(XZ{1,j})+length(impCharX)-1;
        rozsahX = -(length(impCharX)-1):(pocetPrvkuX-length(impCharX));
        pocetPrvkuZ = length(XZ{2,j})+length(impCharZ)-1;
        rozsahZ = -(length(impCharZ)-1):(pocetPrvkuZ-length(impCharZ));
        
        % Graf:
        subplot(6,2,2*j-1)
        plot(rozsahX,upravaKorelaceX)
        title(['Signál zrychlení v ose X ',num2str(j)])
        xlim([min(rozsahX)-10,max(rozsahX)+10])
        ylim([-0.05*max(upravaKorelaceX),1.1*max(upravaKorelaceX)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
        
        subplot(6,2,2*j)
        plot(rozsahZ,upravaKorelaceZ)
        title(['Signál zrychlení v ose Z ',num2str(j)])
        xlim([min(rozsahZ)-10,max(rozsahZ)+10])
        ylim([-0.05*max(upravaKorelaceZ),1.1*max(upravaKorelaceZ)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
    end
    % Graf imp.char.:
    figure(102)
    subplot(6,2,2*i-1)
    plot(impCharX)
    title(['Impulzní charakteristika X ', num2str(i)])
    xlim([-100,length(impCharX)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')
    
    subplot(6,2,2*i)
    plot(impCharZ)
    title(['Impulzní charakteristika Z ', num2str(i)])
    xlim([-100,length(impCharZ)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')
end

% Pro signály X,Z - filtrované:
for i = 1:6
    impCharX = XZFiltPF{1,i};
    impCharZ = XZFiltPF{2,i};
    figure('Position',[0,0,1000,750])
    sgtitle(['Impulzní charakteristiky X,Z ',num2str(i)])
    for j = 1:6
        korelaceX = conv(XZFilt{1,j},impCharX);
        korelaceZ = conv(XZFilt{2,j},impCharZ);
        upravaKorelaceX = abs(korelaceX);
        upravaKorelaceZ = abs(korelaceZ);
        pocetPrvkuX = length(XZFilt{1,j})+length(impCharX)-1;
        rozsahX = -(length(impCharX)-1):(pocetPrvkuX-length(impCharX));
        pocetPrvkuZ = length(XZFilt{2,j})+length(impCharZ)-1;
        rozsahZ = -(length(impCharZ)-1):(pocetPrvkuZ-length(impCharZ));
        
        % Graf:
        subplot(6,2,2*j-1)
        plot(rozsahX,upravaKorelaceX)
        title(['Filtrovaný signál zrychlení v ose X ',num2str(j)])
        xlim([min(rozsahX)-10,max(rozsahX)+10])
        ylim([-0.05*max(upravaKorelaceX),1.1*max(upravaKorelaceX)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
        
        subplot(6,2,2*j)
        plot(rozsahZ,upravaKorelaceZ)
        title(['Filtrovaný signál zrychlení v ose Z ',num2str(j)])
        xlim([min(rozsahZ)-10,max(rozsahZ)+10])
        ylim([-0.05*max(upravaKorelaceZ),1.1*max(upravaKorelaceZ)])
        xlabel('Index posunu')
        ylabel('Amp. kor. fce')
    end
    % Graf imp.char.:
    figure(103)
    subplot(6,2,2*i-1)
    plot(impCharX)
    title(['Impulzní charakteristika X ', num2str(i)])
    xlim([-100,length(impCharX)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')
    
    subplot(6,2,2*i)
    plot(impCharZ)
    title(['Impulzní charakteristika Z ', num2str(i)])
    xlim([-100,length(impCharZ)+100])
    xlabel('Číslo vzorku')
    ylabel('Amplituda')
end