%% FILTRACE PRO ZVÝRAZNĚNÍ FREKVENCÍ UCHOPOVÁNÍ OVLADAČE:
% Skript pro tvorbu odhadu obálky signálů, v nichž se vyskytuje uchopení ovladače,
% a to pro zvýraznění úseků uchopování


clear all, clc

%% 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);
% XYZ = {x1,x2,x3,x4,x5,x6;y1,y2,y3,y4,y5,y6;z1,z2,z3,z4,z5,z6};    % slouží pro porovnání různých mezních frekvencí filtrů

%% 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};      % slouží pro porovnání různých mezních frekvencí filtrů
%}

%% Testovací signál pro specificitu:
test = signaly.B23;
[signalT,xT,yT,zT,casT] = upravSignal(test,0);
aT = vyslednyVektor(xT,yT,zT,casT,0);


%% Výběr signálu:
X = xT;
Y = yT;
Z = zT;
A = aT;
cas = casT;

%% Různé mezní frekvence pro HP a DP:
%{
%% Filtrace HP:
fMHP = [2,3,4,6,8];
for r = 1:numel(fMHP)
    
    XYZHP = cell(3,6);
    AHP = cell(1,6);
    
    % Parametry filtru:
    fVz = 71.07;
    n = 20;
    mezniFrek = fMHP(r)/(fVz/2);
    a = 1;
    b = fir1(n,mezniFrek,'high');

    for i = 1:6
        for j = 1:3
            XYZHP{j,i} = filter(b,a,XYZ{j,i});
        end
        AHP{1,i} = filter(b,a,A{1,i});
    end

    %% Umocnění:
    XYZ2 = cell(3,6);
    A2 = cell(1,6);
    for i = 1:6
        for j = 1:3
            XYZ2{j,i} = XYZHP{j,i}.^2;
        end
        A2{1,i} = AHP{1,i}.^2;
    end
    
    %% Filtrace DP:
    fMDP = [0.5,1,2,3];
    for s = 1:numel(fMDP)
        XYZ2DP = cell(3,6);
        A2DP = cell(1,6);

        n = 20;
        mezniFrek = fMDP(s)/(fVz/2);
        a = 1;
        b = fir1(n,mezniFrek,'low');

        for i = 1:6
            for j = 1:3
                XYZ2DP{j,i} = filter(b,a,XYZ2{j,i});
            end
            A2DP{1,i} = filter(b,a,A2{1,i});
        end

        %% Vykreslení srovnání pro všech 6 signálů:
        cas = {cas1,cas2,cas3,cas4,cas5,cas6};
        xyz = ['X','Y','Z'];

        for i = 1:6
            figure
            for j = 1:3
                nazev = ['Číslo signálu ',num2str(i),', f_{HP} ',num2str(fMHP(r)),' Hz, f_{DP} ',num2str(fMDP(s)),' Hz'];
                sgtitle(nazev)
                
                % Původní:
                subplot(3,2,2*j-1)
                plot(cas{i},XYZ{j,i})
                title(['Původní - ',xyz(j)])
                ylim([-max([max(XYZ{j,i})+10,abs(min(XYZ{j,i})-10)]),max([max(XYZ{j,i})+10,abs(min(XYZ{j,i})-10)])])
                xlabel('Čas [s]')
                ylabel('Zrychlení [m.s^{-2}]')

                % Filtrované:
                subplot(3,2,2*j)
                plot(cas{i},XYZ2DP{j,i})
                title(['Filtrovaný - ',xyz(j)])
                ylim([-max([max(XYZ2DP{j,i})+10,abs(min(XYZ2DP{j,i})-10)]),max([max(XYZ2DP{j,i})+10,abs(min(XYZ2DP{j,i})-10)])])
                xlabel('Čas [s]')
                ylabel('Zrychlení [m.s^{-2}]')
            end
            % saveas(gcf,['cisloSig',num2str(i),'_Fhp',num2str(fMHP(r)),'Fdp',num2str(fMDP(s)),'.png']);
            close all
        end
    end
end
%}

%% Obálka signálu:
% Filtrace HP:
fMHP = 6;
fVz = 71.07;
n = 20;
mezniFrek = fMHP/(fVz/2);
a = 1;
b = fir1(n,mezniFrek,'high');

xHP = filter(b,a,X);
yHP = filter(b,a,Y);
zHP = filter(b,a,Z);
aHP = filter(b,a,A);

% Umocnění:
x2 = xHP.^2;
y2 = yHP.^2;
z2 = zHP.^2;
a2 = aHP.^2;

% Filtrace DP:
fMDP = 3;
fVz = 71.07;
n = 20;
mezniFrek = fMDP/(fVz/2);
a = 1;
b = fir1(n,mezniFrek,'low');

xDP = filter(b,a,x2);
yDP = filter(b,a,y2);
zDP = filter(b,a,z2);
aDP = filter(b,a,a2);

% Vykreslení:
% Původní:
subplot(3,2,1)
plot(cas,X)
title('Původní - osa X')
ylim([-max([max(X)+10,abs(min(X)-10)]),max([max(X)+10,abs(min(X)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')

subplot(3,2,3)
plot(cas,Y)
title('Původní - osa Y')
ylim([-max([max(Y)+10,abs(min(Y)-10)]),max([max(Y)+10,abs(min(Y)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')

subplot(3,2,5)
plot(cas,Z)
title('Původní - osa Z')
ylim([-max([max(Z)+10,abs(min(Z)-10)]),max([max(Z)+10,abs(min(Z)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')

% Filtrované:
subplot(3,2,2)
plot(cas,xDP)
title('Filtrovaný - osa X')
ylim([-max([max(xDP)+10,abs(min(xDP)-10)]),max([max(xDP)+10,abs(min(xDP)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')

subplot(3,2,4)
plot(cas,yDP)
title('Filtrovaný - osa Y')
ylim([-max([max(yDP)+10,abs(min(yDP)-10)]),max([max(yDP)+10,abs(min(yDP)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')

subplot(3,2,6)
plot(cas,zDP)
title('Filtrovaný - osa Z')
ylim([-max([max(zDP)+10,abs(min(zDP)-10)]),max([max(zDP)+10,abs(min(zDP)-10)])])
xlabel('Čas [s]')
ylabel('Zrychlení [m.s^{-2}]')