%% INTEGRACE:
% Skript pro tvorbu algoritmu trasování pohybu ovladače

% {
clear all, close all, clc
signaly = load('signalyIntegrace.mat');
nazvySignalu = fieldnames(signaly);
%}

vyber = nazvySignalu{1,:};
prubeh = signaly.(vyber);

%% Úprava raw dat z měřícího přípravku:
[signal,x,y,z,cas] = upravSignal(prubeh,0);


%% Vektor zrychlení v prostoru:
a = vyslednyVektor(x,y,z,cas,0);      % výpočet velikosti výsledného zrychlení ovladače

%{
%% Offset tíhové síly:                % první možnost eliminace tíhové síly
offsetA = mean(a);
offsetX = mean(x);
offsetY = mean(y);
offsetZ = mean(z);

aNove = a - offsetA;
xNove = x - offsetX;
yNove = y - offsetY;
zNove = z - offsetZ;
%}

% {
%% Filtrace HP pro odstranění stejnosměrné složky (tíhová síla):
% druhá možnost eliminace tíhové síly

fVz = 71.07;    % vzorkovací frekvence
n = 20;         % řád filtru HP
fM= 15;         % mezní frekvence

[aNove,xNove,yNove,zNove] = filtraceHP(a,x,y,z,cas,fVz,n,fM,0);

% Vymazání chybových začátků signálů (náběh filtru):
aNove = aNove(n+1:end);
xNove = xNove(n+1:end);
yNove = yNove(n+1:end);
zNove = zNove(n+1:end);

cas = cas(n+1:end);   % odpovídající čas
%}

%% Výpočet rychlosti (první integrace):
vA = cumtrapz(cas,aNove);
vX = cumtrapz(cas,xNove);
vY = cumtrapz(cas,yNove);
vZ = cumtrapz(cas,zNove);

%% Výpočet dráhy (druhá integrace):
sA = cumtrapz(cas,vA);
sX = cumtrapz(cas,vX);
sY = cumtrapz(cas,vY);
sZ = cumtrapz(cas,vZ);

s = [sA(end),sqrt(sX(end)^2+sY(end)^2+sZ(end)^2)]   % dráha vypočtená z výsledného zrychlení a z os x, y, z

% {
%% Grafy rychlostí a drah:
% Graf pro vypočtenou rychlost z výsledného zrychlení:
figure('Position',[20 20 1000 750])

mezX = cas(end) + 1;
mezY = max([max(vA),abs(min(vA))])*1.5;

plot(cas,vA)
title('Rychlost vypočtená z výsledného zrychlení')
xlabel('Čas [s]')
ylabel('Rychlost [m.s^{-1}]')
xlim([0,mezX])
ylim([-mezY,mezY])

% Graf pro vypočtené rychlosti ze zrychlení v osách:
figure('Position',[20 20 1000 750])

mezX = cas(end) + 1;
mezY = max([max(vX),abs(min(vX)),max(vY),abs(min(vY)),max(vZ),abs(min(vZ))])*1.5;

subplot 311
plot(cas,vX)
title('Rychlost vypočtená ze zrychlení v ose X')
xlabel('Čas [s]')
ylabel('Rychlost [m.s^{-1}]')
xlim([0,mezX])
ylim([-mezY,mezY])

subplot 312
plot(cas,vY)
title('Rychlost vypočtená ze zrychlení v ose Y')
xlabel('Čas [s]')
ylabel('Rychlost [m.s^{-1}]')
xlim([0,mezX])
ylim([-mezY,mezY])

subplot 313
plot(cas,vZ)
title('Rychlost vypočtená ze zrychlení v ose Z')
xlabel('Čas [s]')
ylabel('Rychlost [m.s^{-1}]')
xlim([0,mezX])
ylim([-mezY,mezY])

% Graf pro vypočtenou dráhu z výsledné rychlosti:
figure('Position',[10 10 1000 750])

mezX = cas(end) + 1;
mezY = max([max(sA),abs(min(sA))])*1.5;

plot(cas,sA)
title('Dráha vypočtená z výsledné rychlosti')
xlabel('Čas [s]')
ylabel('Dráha [m]')
xlim([0,mezX])
ylim([-mezY,mezY])

% Graf pro vypočtenou dráhu z rychlostí v osách:
figure('Position',[10 10 1000 750])

mezX = cas(end) + 1;
mezY = max([max(sX),abs(min(sX)),max(sY),abs(min(sY)),max(sZ),abs(min(sZ))])*1.5;

subplot 311
plot(cas,sX)
title('Dráha vypočtená z rychlosti v ose X')
xlabel('Čas [s]')
ylabel('Dráha [m]')
xlim([0,mezX])
ylim([-mezY,mezY])

subplot 312
plot(cas,sY)
title('Dráha vypočtená z rychlosti v ose Y')
xlabel('Čas [s]')
ylabel('Dráha [m]')
xlim([0,mezX])
ylim([-mezY,mezY])

subplot 313
plot(cas,sZ)
title('Dráha vypočtená z rychlosti v ose Z')
xlabel('Čas [s]')
ylabel('Dráha [m]')
xlim([0,mezX])
ylim([-mezY,mezY])
%}