%% vycisteni premennych a command window
clear all; close all; clc

%% Nacitanie dat z tabulky excel
%Nacitanie udajov zo Sense-it
tab = xlsread('HTC_ctverec_stul_stredne_0.6x0.6m.xlsx'); 
time = tab(2:end,1)';
acc_x = tab(2:end,2)';
acc_y = tab(2:end,3)';
acc_z = tab(2:end,4)';

%Nacitanie z EMG
%2 g: st?pce 2,6,10
%4 g: st?pce 3,7,11
%8 g: st?pce 4,8,12
%16 g: st?pce 5,9,13

% tab = xlsread('EMG_ctverec_zem_rychle_0.6x0.6m.xlsx');  
% time = tab(2:end,1)'/1000; %vektor casu je z emg povodne nastaveny v milisekundach (prevod na sekundy)
% acc_x = tab(2:end,3)'*9.81; %hodnoty zrychlenia su v jednotkach g (prevod *9.81 na m/s^2)
% acc_y = tab(2:end,7)'*9.81;
% acc_z = tab(2:end,11)'*9.81;


deltaT=time(2)-time(1);% vzorkovacia perioda signalu
fvz=1/deltaT; % vzorkovacia frekvencia signalu

%% Vypocet rychlosti v jednotlivych osach
% vypocet vektoru rychlosti pomoci 'trapz' - lichobeznikova integrace
k=fvz/10;
r=rem(length(acc_x),k);%zbytok po deleni k (integrujeme vzdy k po sebe iducich hodnot)
p=length(acc_x)-r; %p=dlzka vektoru vstupujuceho do cyklu

% vypocet vektoru rychlosti v ose x
a=1;
while a<=(p-k)
    pom = trapz(time(a:(a+k)),acc_x(a:(a+k)));
    vel_x(a:(a+k))=pom;
    a=a+k;
end 
if r==0
    vel_x=vel_x;
elseif r~=0;
    pom2=trapz(time(p:end),acc_x(p:end));
    vel_x=[vel_x,pom2];
end 

% vypocet vektoru rychlosti v ose y
a=1;
while a<=(p-k)
    pom = trapz(time(a:(a+k)),acc_y(a:(a+k)));
    vel_y(a:(a+k))=pom;
    a=a+k;
end 
if r==0
    vel_y=vel_y;
elseif r~=0;
    pom2=trapz(time(p:end),acc_y(p:end));
    vel_y=[vel_y,pom2];
end 

% vypocet vektoru rychlosti v ose z
a=1;
while a<=(p-k)
    pom = trapz(time(a:(a+k)),acc_z(a:(a+k)));
    vel_z(a:(a+k))=pom;
    a=a+k;
end

if r==0
    vel_z=vel_z;
elseif r~=0;
    pom2=trapz(time(p:end),acc_z(p:end));
    vel_z=[vel_z,pom2];
end 
time_v=time(1:length(vel_x));

%% najdenie hranic a vrcholovych hodnot jednotlivych pohybovych elementov 

% zavedenie absolutnej hodnoty vo vektore rychlosti vel_x
vel_x_a=abs(vel_x);
vel_y_a=abs(vel_y);
vel_z_a=abs(vel_z);

% vyhladenie signalu medianovym filtrom, okno filtru je zavisle na vzorkovacej frekvencii
vel_x_m=medfilt1(vel_x,(0.2*fvz));
vel_y_m=medfilt1(vel_y,(0.2*fvz));
vel_z_m=medfilt1(vel_z,(0.2*fvz));

% deklaracia novej premennej vel_x_s s ktorou program dalej pracuje
vel_x_s=vel_x_a;
vel_y_s=vel_y_a;
vel_z_s=vel_z_a;

%///////////////////////////////////////////////////////////////OSA X///////////////////////////////////////////////////////////////

[pks_x,locs_x] = findpeaks(vel_x_s); %najdenie peakov signalu
b=round(sum(vel_x_a)/length(vel_x_a)); %najdenie hodnoty okolo ktorej vektor rychlosti osciluje

if b~=0
    vel_x_s(vel_x_s<=(b+0.2*(b-max(pks_x(pks_x<max(pks_x))))))=b; %odstranenie hodnot nizsich nez .2 nasobok druhej najvyssej hodnoty vektoru
    vel_x_s(vel_x_s>=(b-0.2*(b-max(pks_x(pks_x<max(pks_x))))))=b;
else
    vel_x_s(vel_x_s<=(0.2*max(pks_x(pks_x<max(pks_x)))))=0; %odstranenie hodnot nizsich nez .1 nasobok druhej najvyssej hodnoty vetoru
end 
    vel_x_s=medfilt1(vel_x_s,(0.2*fvz)); %vyhladenie signalu vytvorenim jeho obalky (medianovy filter, velkost okna je .2 nasobok vzorkovacej frekvencie)

%najdenie hranic pohybovych elementov
MP_x=[]; %zaciatok pohyboveho elementu
for i=1:(length(vel_x_s)-1)
    if (vel_x_s(i)==b & vel_x_s(i+1)>b)
        MP_x=[MP_x,i];
    end
end

NP_x=[]; %koniec pohyboveho elementu
for i=2:(length(vel_x_s))
    if (vel_x_s(i)==b & vel_x_s(i-1)>b)
        NP_x=[NP_x,i];
    end
end

% osetrenie vektorov MP_x, NP_x pre dalsie spracovanie, ponechanie iba
% dvojic hodnot, ktore oznacuju hranice konkretnych elementov

if length(MP_x)~=length(NP_x)
a=min(length(MP_x), length(NP_x));
for i=1:a
    if MP_x(i)<NP_x(i)
        MP_x(i)=MP_x(i);
        NP_x(i)=NP_x(i);
    else 
        MP_x(i)=0;
        NP_x(i)=0;
    end
end
MP_x(MP_x==0)=[];
NP_x(NP_x==0)=[];

a=min(length(MP_x), length(NP_x));
MP_x=MP_x(1:a);
NP_x=NP_x(1:a);
end

%najdenie vrcholovych hodnot pohybovych elementov
M_x=[];
N_x=[];

for i=1:length(MP_x)
    pom1=MP_x(i);
    pom2=NP_x(i);
    px=vel_x_s(pom1:pom2);
    [a,c]=findpeaks(px);
    c(a==0)=[];
    a(a==0)=[];
    c2=[];
    
    %kazdy pohybovy element musi obsahovat prave 2 vrcholy 
    if length(a)==2 
        M_x=[M_x,a]; 
        c=c+pom1;
        N_x=[N_x,c];
        
    elseif length(a)==1
        [a1,c1]=max(px(px<a));
        if a1==0
            a=[];
            c=[];
            MP_x(i)=0;
            NP_x(i)=0;
        else 
            M_x=[M_x,a,a1];
            c=[c,c1];
            c=c+pom1;
            N_x=[N_x,c];
        end
        
    elseif length(a)>2 %ak boli v pohybovom elemente najdene viac nez 2 vrcholy, program si pamata len 2 najvacsie
        pa=unique(a);
        a2 = pa([end-1,end]);
        for i=1:length(a2)
            k=(a==a2(i));
            if sum(k)==1
                c2(i)=c(k);
            else
                [u,v]=max(k);
                c2(i)=c(v);
            end
        end
        c2=c2+pom1;
        N_x=[N_x,c2];
        M_x=[M_x,a2];
    end   
end

NP_x(MP_x==0)=[];%odstranenie dvojic hranic, medzi ktorymi je len 1 vrchol - tento usek nie je identifikovany ako pohybovy element
MP_x(MP_x==0)=[];

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%///////////////////////////////////////////////////////////////OSA Y///////////////////////////////////////////////////////////////

[pks_y,locs_y] = findpeaks(vel_y_s); %najdenie peakov signalu
b=round(sum(vel_y_a)/length(vel_y_a));%najdenie hodnoty okolo ktorej vektor rychlosti osciluje
if b~=0
    vel_y_s(vel_y_s<=(b+0.2*(b-max(pks_y(pks_y<max(pks_y))))))=b; %odstranenie hodnot nizsich nez .2 nasobok druhej najvyssej hodnoty vektoru
    vel_y_s(vel_y_s>=(b-0.2*(b-max(pks_y(pks_y<max(pks_y))))))=b;
else
    vel_y_s(vel_y_s<=(0.2*max(pks_y(pks_y<max(pks_y)))))=0; %odstranenie hodnot nizsich nez .2 nasobok druhej najvyssej hodnoty vektoru
end 
    vel_y_s=medfilt1(vel_y_s,(0.2*fvz)); %vyhladenie signalu vytvorenim jeho obalky (medianovy filter, velkost okna je .2 nasobok vzorkovacej frekvencie

%najdenie hranic pohybovych elementov
MP_y=[];
for i=1:(length(vel_y_s)-1)
    if (vel_y_s(i)==b & vel_y_s(i+1)>b)
        MP_y=[MP_y,i];
    end
end

NP_y=[];
for i=2:(length(vel_y_s))
    if (vel_y_s(i)==b & vel_y_s(i-1)>b)
        NP_y=[NP_y,i];
    end
end

% osetrenie vektorov MP_y, NP_y pre dalsie spracovanie, ponechanie iba
% dvojic hodnot, ktore oznacuju hranice konkretnych elementov

a=min(length(MP_y), length(NP_y));
for i=1:a
    if MP_y(i)<NP_y(i)
        MP_y(i)=MP_y(i);
        NP_y(i)=NP_y(i);
    else 
        MP_y(i)=0;
        NP_y(i)=0;
    end
end
MP_y(MP_y==0)=[];
NP_y(NP_y==0)=[];

a=min(length(MP_y), length(NP_y));
MP_y=MP_y(1:a);
NP_y=NP_y(1:a);

%najdenie vrcholovych hodnot pohybovych elementov
M_y=[];
N_y=[];


for i=1:length(MP_y)
    pom1=MP_y(i);
    pom2=NP_y(i);
    py=vel_y_s(pom1:pom2);
    [a,c]=findpeaks(py);
    c(a==0)=[];
    a(a==0)=[];
    c2=[];
    
    %kazdy pohybovy element musi obsahovat prave 2 vrcholy 
    if length(a)==2 
        M_y=[M_y,a]; 
        c=c+pom1;
        N_y=[N_y,c];
        
    elseif length(a)==1
        [a1,c1]=max(py(py<a));
        if a1==0
            a=[];
            c=[];
            MP_y(i)=0;
            NP_y(i)=0;
        else 
            M_y=[M_y,a,a1];
            c=[c,c1];
            c=c+pom1;
            N_y=[N_y,c];
        end
        
    elseif length(a)>2 %ak boli v pohybovom elemente najdene viac nez 2 vrcholy, program si pamata len 2 najvacsie
        pa=unique(a);
        a2 = pa([end-1,end]);
        for i=1:length(a2)
            k=(a==a2(i));
            if sum(k)==1
                c2(i)=c(k);
            else
                [u,v]=max(k);
                c2(i)=c(v);
            end
        end
        c2=c2+pom1;
        N_y=[N_y,c2];
        M_y=[M_y,a2];
    end   
end

NP_y(MP_y==0)=[];%odstranenie dvojic hranic, medzi ktorymi je len 1 vrchol - tento usek nie je identifikovany ako pohybovy element
MP_y(MP_y==0)=[];

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%///////////////////////////////////////////////////////////////OSA Z///////////////////////////////////////////////////////////////

[pks_z,locs_z] = findpeaks(vel_z_s); %najdenie peakov signalu
b=round(sum(vel_z_a)/length(vel_z_a)); %najdenie hodnoty okolo ktorej vektor rychlosti osciluje

if b~=0
    vel_z_s(vel_z_s<=(b+0.2*(b-max(pks_z(pks_z<max(pks_z))))))=b; %odstranenie hodnot nizsich nez .2 nasobok druhej najvyssej hodnoty vektoru
    vel_z_s(vel_z_s>=(b-0.2*(b-max(pks_z(pks_z<max(pks_z))))))=b;
else
    vel_z_s(vel_z_s<=(0.2*max(pks_z(pks_z<max(pks_z)))))=0; %odstranenie hodnot nizsich nez .2 nasobok druhej najvyssej hodnoty vektoru
end 
    vel_z_s=medfilt1(vel_x_s,(0.2*fvz)); %vyhladenie signalu vytvorenim jeho obalky (medianovy filter, velkost okna je .2 nasobok vzorkovacej frekvencie


%najdenie hranic pohybovych elementov
MP_z=[];
for i=1:(length(vel_z_s)-1);
    if (vel_z_s(i)==b & vel_z_s(i+1)>b);
        MP_z=[MP_z,i];
    end
end

NP_z=[];
for i=2:(length(vel_z_s))
    if (vel_z_s(i)==b & vel_z_s(i-1)>b);
        NP_z=[NP_z,i];
    end
end


% osetrenie vektorov MP_z, NP_z pre dalsie spracovanie, ponechanie iba
% dvojic hodnot, ktore oznacuju hranice konkretnych elementov

a=min(length(MP_z), length(NP_z));
for i=1:a
    if MP_z(i)<NP_z(i);
        MP_z(i)=MP_z(i);
        NP_z(i)=NP_z(i);
    else 
        MP_z(i)=0;
        NP_z(i)=0;
    end
end
MP_z(MP_z==0)=[];
NP_z(NP_z==0)=[];

a=min(length(MP_z), length(NP_z));
MP_z=MP_z(1:a);
NP_z=NP_z(1:a);

%najdenie vrcholovych hodnot pohybovych elementov
M_z=[];
N_z=[];

for i=1:length(MP_z)
    pom1=MP_z(i);
    pom2=NP_z(i);
    pz=vel_z_s(pom1:pom2);
    [a,c]=findpeaks(pz);
    c(a==0)=[];
    a(a==0)=[];
    c2=[];
    
    %kazdy pohybovy element musi obsahovat prave 2 vrcholy 
    if length(a)==2 
        M_z=[M_z,a]; 
        c=c+pom1;
        N_z=[N_z,c];
        
    elseif length(a)==1
        [a1,c1]=max(pz(pz<a));
        if a1==0
            a=[];
            c=[];
            MP_z(i)=0;
            NP_z(i)=0;
        else 
            M_z=[M_z,a,a1];
            c=[c,c1];
            c=c+pom1;
            N_z=[N_z,c];
        end
        
    elseif length(a)>2 %ak boli v pohybovom elemente najdene viac nez 2 vrcholy, program si pamata len 2 najvacsie
        pa=unique(a);
        a2 = pa([end-1,end]);
        for i=1:length(a2)
            k=(a==a2(i));
            if sum(k)==1
                c2(i)=c(k);
            else
                [u,v]=max(k);
                c2(i)=c(v);
            end
        end
        c2=c2+pom1;
        N_z=[N_z,c2];
        M_z=[M_z,a2];
    end   
end

NP_z(MP_z==0)=[];%odstranenie dvojic hranic, medzi ktorymi je len 1 vrchol - tento usek nie je identifikovany ako pohybovy element
MP_z(MP_z==0)=[];

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%% uprava vektoru rychlosti spravnym smerom - otocenie pohybovych elementov v smere prveho vykmitu 

%///////////////////////////////////////////////////////////////OSA X///////////////////////////////////////////////////////////////

vel_x_k=vel_x;
[o,w]=sort(N_x);
N_x=o;
M_x=M_x(w);

pomM_x=M_x(1:2:end);
pomN_x=N_x(1:2:end);
for i=1:(length(pomM_x))
    if sign(vel_x_s(pomN_x(i)))==sign(vel_x_m(pomN_x(i))) 
         vel_x_k(MP_x(i):NP_x(i))= vel_x_a(MP_x(i):NP_x(i));
    else 
         vel_x_k(MP_x(i):NP_x(i))= -vel_x_a(MP_x(i):NP_x(i));
    end 
end

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%///////////////////////////////////////////////////////////////OSA Y///////////////////////////////////////////////////////////////

vel_y_k=vel_y;
[o,w]=sort(N_y);
N_y=o;
M_y=M_y(w);

pomM_y=M_y(1:2:end);
pomN_y=N_y(1:2:end);
for i=1:(length(pomM_y))
    if sign(vel_y_s(pomN_y(i)))==sign(vel_y_m(pomN_y(i))) 
         vel_y_k(MP_y(i):NP_y(i))= vel_y_a(MP_y(i):NP_y(i));
    else 
         vel_y_k(MP_y(i):NP_y(i))= -vel_y_a(MP_y(i):NP_y(i));
    end 
end

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%///////////////////////////////////////////////////////////////OSA Z//////////////////////////////////////////////////////////////

vel_z_k=vel_z;
[o,w]=sort(N_z);
N_z=o;
M_z=M_z(w);

pomM_z=M_z(1:2:end);
pomN_z=N_z(1:2:end);
for i=1:(length(pomM_z))
    if sign(vel_z_s(pomN_z(i)))==sign(vel_z_m(pomN_z(i))) 
         vel_z_k(MP_z(i):NP_z(i))= vel_z_a(MP_z(i):NP_z(i));
    else 
         vel_z_k(MP_z(i):NP_z(i))= -vel_z_a(MP_z(i):NP_z(i));
    end 
end

%///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

%% vypocet  vektoru pozice kumulativnou lichobeznikovou integraciou 

pos_x = cumtrapz(time_v,vel_x_k);
pos_y = cumtrapz(time_v,vel_y_k);
pos_z = cumtrapz(time_v,vel_z_k);

%% Zobrazeni 
% % zrychleni
% figure(1)
% subplot (3,1,1)
% plot(time, acc_x); 
% title ('Graf zavislosti zrychlenia na case')
% xlabel('time (s)')
% ylabel('X Acc (m/s^2)')
% subplot (3,1,2)
% plot (time, acc_y);
% xlabel('time (s)')
% ylabel('Y Acc (m/s^2)')
% subplot (3,1,3)
% plot (time, acc_z); 
% xlabel('time (s)')
% ylabel('Z Acc (m/s^2)')
% 
% %rychlost
% figure(2)
% subplot (3,1,1)
% plot(time_v, vel_x); 
% title ('Graf zavislosti rychlosti na case')
% xlabel('time (sec)')
% ylabel('X Acceleration (m/sec^2)')
% subplot (3,1,2)
% plot (time_v, vel_y);
% xlabel('time (sec)')
% ylabel('Y Acceleration (m/sec^2)')
% subplot (3,1,3)
% plot (time_v, vel_z); 
% xlabel('time (sec)')
% ylabel('Z Acceleration (m/sec^2)')
% 
% %rychlost v absolutni hodnote
% figure(3) 
% subplot (3,1,1)
% plot (time_v,vel_x_a);
% title ('Graf zavislosti rychlosti v absolutni hodnote na case')
% xlabel('time (sec)')
% ylabel('X Velocity (m/sec)')
% subplot (3,1,2)
% plot (time_v, vel_y_a); 
% xlabel('time (sec)')
% ylabel('Y Velocity (m/sec)')
% subplot (3,1,3)
% plot (time_v, vel_z_a); 
% xlabel('time (sec)')
% ylabel('Z Velocity (m/sec)')
% 
% %filtrovana rychlost
% figure(4)
% subplot (3,1,1)
% plot (time_v,vel_x_s);
% hold on 
% 
% TF = isempty(MP_x);
% if TF==0
% plot (MP_x/fvz,0,'r*')
% hold on 
% end
% 
% TF = isempty(NP_x);
% if TF==0
% plot (NP_x/fvz,0,'g*')
% hold on 
% end 
% TF = isempty(N_x);
% if TF==0
% plot (N_x/fvz,M_x,'y*')
% hold off
% end
% 
% title ('Graf ?asovej zavislosti filtrovanej rychlosti')
% xlabel('time (sec)')
% ylabel('X Velocity (m/sec)')
% 
% subplot (3,1,2)
% plot (time_v,vel_y_s);
% hold on 
% 
% TF = isempty(MP_y);
% if TF==0
% plot (MP_y/fvz,0,'r*')
% hold on 
% end
% 
% TF = isempty(NP_y);
% if TF==0
% plot (NP_y/fvz,0,'g*')
% hold on 
% end
% 
% TF = isempty(N_y);
% if TF==0
% plot (N_y/fvz,M_y,'y*')
% hold off
% end
% xlabel('time (sec)')
% ylabel('Y Velocity (m/sec)')
% 
% subplot (3,1,3)
% plot (time_v,vel_z_s);
% hold on 
% 
% TF = isempty(MP_z);
% if TF==0
% plot (MP_z/fvz,0,'r*')
% hold on 
% end
% 
% TF = isempty(NP_z);
% if TF==0
% plot (NP_z/fvz,0,'g*')
% hold on 
% end 
% 
% TF = isempty(N_z);
% if TF==0
% plot (N_z/fvz,M_z,'y*')
% hold off
% end 
% 
% xlabel('time (sec)')
% ylabel('Z Velocity (m/sec)')
% 
% %vystupna rychlost
% figure(5) 
% subplot (3,1,1)
% plot (time_v,vel_x_k);
% title ('Graf zavislosti rychlosti na case - konecny vektor')
% xlabel('time (sec)')
% ylabel('X Velocity (m/sec)')
% subplot (3,1,2)
% plot (time_v, vel_y_k); 
% xlabel('time (sec)')
% ylabel('Y Velocity (m/sec)')
% subplot (3,1,3)
% plot (time_v, vel_z_k); 
% xlabel('time (sec)')
% ylabel('Z Velocity (m/sec)')
% 
% % pozice
% figure(6)
% subplot (3,1,1)
% plot (time_v,pos_x);
% title ('Graf zavislosti drahy na case')
% xlabel('time (sec)')
% ylabel('X Possition (m)')
% subplot (3,1,2)
% plot (time_v, pos_y); 
% xlabel('time (sec)')
% ylabel('Y Possition (m)')
% subplot (3,1,3)
% plot (time_v, pos_z); 
% xlabel('time (sec)')
% ylabel('Z Possition (m)')
 
% 3D
figure (7)
plot3 (pos_x,pos_y,pos_z)
title ('Zobrazenie pohybu v 3D priestore')
xlabel('Position (x)')
ylabel('Position (y)')
zlabel('Position (z)')

% 2D projekcia
figure (8)
plot(pos_x,pos_y)
title ('Zobrazenie pohybu v 2D priestore - projekcia X-Y')
xlabel('Position (x)')
ylabel('Position (y)')

