clc;
clear all;
close all;

%nacteni vstupniho souboru dat 12 svodu
load('MO1_001_12.mat')%x je matice, kde radky odpovidaji svodum
load('poziceQRSvCSEv2.mat')
%nacteni signalu
time = (1:5000)/500;

figure
for i=1:3
    subplot(3,1,i)
    plot(time,x(i,:))
    xlabel('cas,[s]')
    ylabel('U,[µv]') 
end

%---------------------%zakladni filtrace signalu---------------------------

fvz=500;
a=1;
b1 = fir1(1500,[6 18]/(fvz/2), 'bandpass'); % jestli do toho zapojime pomaly pohyb pacienta, musime zvysit hodnotu do 2 Hz
a2= 8/fvz;
b2 = [1 2 0 -2 -1];
b3 = fir1(1000, 10/(fvz/2), 'low');
for i=1:12
   y_QRS(i,:) = filtfilt(b1,a,x(i,:));%FIR filtr na pasmovou propust 0,8-18 Hz pro odstraneni driftu, sitoveho brumu a vzsokych frekvenci
   y_dif(i,:) = diff(y_QRS(i,:));%derivace signalu pro zjisteni sklonu-strnosti hran, koeficienty jsou vybrane podle clanku Pan a Tomkins
   y_2(i,:) = y_dif(i,:).^2;%umocneni signalu na druhou pro zvyrazneni komplexu QRS
   y(i,:) = filtfilt(b3,a,y_2(i,:)); %vzhlazeni filtrem na DP(dalsi moznost vyhlazeni integracnim oknem)
end

figure
for i=1:3
   subplot(3,1,i)
   plot(y(i,:),'b')
end

%---------------------%detekce vlny R v predzpracovanem signalu------------

%detekce vlny R se provadi pomoci nastaveneho adaprtivniho prahu
%je dulezita pro cely rozmereni, protoze je zakladenm pro spravnou detekci
%zbyvajicich vln
vektor_pozice=[];
for i=1:12
    [pks, locs] = findpeaks(y(i,:));
    treshold=3*mean(y(i,:));
    Rka = [];
    Hodnoty_Rka = [];
    for j=1:length(pks)
        if pks(j)>treshold
            Rka= [Rka locs(j)];
            Hodnoty_Rka = [Hodnoty_Rka pks(j)];
        end
    end
    
    final{i,:} = Rka;
    final_hodnoty{i,:} = Hodnoty_Rka;
 
    %-----------prepocet pozic v indexem na casove okamziky----------------
    pozice{i,:}=final{i,:}/fvz;
    vektor_pozice=[vektor_pozice pozice{i,:}];%pozice jsou prepsany do jednoho vektoru pro nasledne zpracovani  
end

figure
for i=1:3
    subplot(3,1,i)
    plot(y(i,:),'b')
    hold on
    stem(final{i,:},final_hodnoty{i,:},'r*')
    xlabel('vzorky,[-]')
    ylabel('U,[µv]')
end

%---------------------hledani globalnich pozic R vln-----------------------

pozice_vysledek = sort(vektor_pozice);%pozice jsou usporadane podle velikosti od nejmensi k nejvetsi
globR = shlukovani(pozice_vysledek);%pomoci shlukove analyzy najdeme globalni pozice R vln

%vykresleni globalnich a lokalnich pozic R do vstupniho signalu x
figure
for i=1:3
    subplot(3,1,i)
    plot(time,x(i,:))
    hold on
    line([pozice{i,:};pozice{i,:}],[1500;1000],'Color','r')
    hold on
    for j=1:length(globR)
    line([globR(j)/fvz;globR(j)/fvz],[1500;1000],'Color','g')
    end
    hold off
end

%---------------------lokalni rozmereni QRS komplexu-----------------------
for i=1:12
    %---------------------zpresneni poloh R vln----------------------------
    pom_R1 = y_dif(i,:); %vezme derivovany signal
    pom_R2 = globR; %vezme detekovane pozice R vln
    for j=1:length(pom_R2) %cyklus projede vsem vektorem detekovanych R, a pro kazde zpresni pozici
        pom_R3 = abs(pom_R1(pom_R2(j)-10:pom_R2(j)+10));
        pom_R4=find(pom_R3==min(pom_R3));
        RR(j) = pom_R2(j)-10+pom_R4-1;%zpresnene polohy R ve vzorcich
    end
    matice_RR{i,:} = RR;
end

    %--------------nalezeni vlny Q-----------------------------------------
    %vlna Q je tady definovana jako prvni pruchod nulou pred vlnou R, s
    %podminkou hodnoty intervalu mensi nez 80 ms neboli 40 vzorku
  vektor_Q = [];
  for i=1:12
    pom_R1 = y_dif(i,:);
    pom_Q1 = matice_RR{i,:}; %vezme detekovane pozice R vln
     for j=1:length(pom_Q1)
        pom_Q2 = abs(pom_R1(pom_Q1(j)-25:pom_Q1(j)-5));%kdyz interval zmenime na 30 dosahneme lepsich vysledku
        pom_Q3=find(pom_Q2==min(pom_Q2));%najdeme index nejblizsi nuly pred R
        QQ(j)= pom_Q1(j)-25+pom_Q3-1; 
     end
     matice_QQ{i,:} = QQ;
     vektor_Q=[vektor_Q matice_QQ{i,:}/fvz];
  end
  
    %--------------nalezeni vlny S-----------------------------------------
    vektor_S = [];
  for i=1:12
      pom_R1 = y_dif(i,:);
      pom_Q1 = matice_RR{i,:};
      for j=1:length(pom_Q1)-1
        pom_S1= abs(pom_R1(pom_Q1(j)+5:pom_Q1(j)+20));
        pom_S2= find(pom_S1==min(pom_S1));
        SS(j)= pom_Q1(j)+5+pom_S2-1;
      end
      matice_SS{i,:} = SS;
      vektor_S=[vektor_S matice_SS{i,:}/fvz];
  end
  
  %-------------------nalezeni zacatku QRS komplexu------------------------
 kq=1.8;%koeficient k pro zacatek vlny Q, predtim 1.8
 vektor_zacatek = [];
 for i=1:12
    pom_zacatek1 = y_dif(i,:);
    pom_zacatek2 = matice_QQ{i,:};
   for j=2:length(pom_zacatek2)
       pom_zacatek3 = abs(pom_zacatek1(pom_zacatek2(j)-40:pom_zacatek2(j)));
       [pks1, locs1] = findpeaks(pom_zacatek3);
       Hq(j) = pks1(end)/kq;
       if locs1(end)>10
           pom_zacatek4 = min(find(pom_zacatek3(locs1(end)-10:locs1(end))>Hq(j)));
           QRS_z(j) = pom_zacatek2(j)-40+locs1(end)-10+pom_zacatek4-1-1;
       else
           QRS_z(j) = pom_zacatek2(j)-40+locs1(end);
       end
   end
   matice_Hq{i,:}= Hq;
   QRS_zacatek{i,:}= QRS_z;
   
   vektor_zacatek = [vektor_zacatek QRS_zacatek{i,:}/fvz];
 end
  
 %-------------------nalezeni koncu QRS komplexu---------------------------
 ks=3;%koeficient k pro konce vlny S
 vektor_konec = [];
 for i=1:12
    pom_konec1 = y_dif(i,:);
    pom_konec2 = matice_SS{i,:};
   for j=1:length(pom_konec2)
       pom_konec3 = abs(pom_konec1(pom_konec2(j):pom_konec2(j)+40));
       [pks1, locs1] = findpeaks(pom_konec3);
       Hs(j) = pks1(1)/kq;
       if (locs1(1)+10)<40
           pom_konec4 = max(find(pom_konec3(locs1(1):locs1(1)+10)>Hs(j)));
           QRS_k(j) = pom_konec2(j)+locs1(1)-1+pom_konec4-1;
       else
           QRS_k(j) = pom_konec2(j)+locs1(1);
       end
   end
   matice_Hs{i,:}= Hs;
   QRS_konec{i,:}= QRS_k;
   
   vektor_konec=[vektor_konec QRS_konec{i,:}/fvz];
 end
 
 %vykresleni vysledku lokalniho rozmereni QRS komplexu
 figure
 for i=1:3
    subplot(3,1,i)
    plot(y_dif(i,:),'b')
    xlabel('vzorky,[-]')
    ylabel('U,[µv]')
    hold on
    line([0;5000],[0;0],'Color','black')
    hold on
    line([matice_RR{i,:};matice_RR{i,:}],[-100;100],'Color','black')
    hold on
    line([matice_QQ{i,:};matice_QQ{i,:}],[-100;100],'Color','r')
    hold on
    line([matice_SS{i,:};matice_SS{i,:}],[-100;100],'Color','r')
    hold on
    line([QRS_zacatek{i,:};QRS_zacatek{i,:}],[-100;100],'Color','g')
    hold on
    line([QRS_konec{i,:};QRS_konec{i,:}],[-100;100],'Color','g')
 end
  
%---------------------globalni rozmereni QRS komplexu----------------------
globQ = shlukovani(sort(vektor_Q));
globS = shlukovani(sort(vektor_S));
glob_zacatekQ = shlukovani(sort(vektor_zacatek));
glob_konecS = shlukovani(sort(vektor_konec));

figure
for i=1:3
    subplot(3,1,i)
    plot(time,x(i,:))
    xlabel('cas,[-]')
    ylabel('U,[µv]')
    hold on
    for j=1:length(glob_zacatekQ)
    line([glob_zacatekQ(j)/fvz;glob_zacatekQ(j)/fvz],[1500;1000],'Color','r')
    end
    hold on
    for j=1:length(glob_konecS)
    line([glob_konecS(j)/fvz;glob_konecS(j)/fvz],[1500;1000],'Color','r')
    end
    hold on
    for j=1:length(globR)
    line([globR(j)/fvz;globR(j)/fvz],[1500;1000],'Color','black')
    end
end

%---------------------lokalni rozmereni vlny P a T-------------------------
%je potreba aplikovat jine predzpracovani signalu z toho duvodu,ze energie
%P a T vln je soustredena na nizsich kmitoctach, nez QRS komplex. Je
%potreba na signal po diference aplikovat dalsi filtr na DP s hornim meznim
%kmitictem kolem 12 Hz
b4 = fir1(1500,[1 11]/(fvz/2), 'bandpass');
for i=1:12
    y_filtr(i,:) = filtfilt(b4,a,x(i,:));
    y_PT(i,:) = diff(y_filtr(i,:));
end

 %-------------------nalezeni vlny P---------------------------------------
%pro prvni vlnu algoritmus nepocita, protoze ve vetsine signalu je prilis
%mala vzdalenost 

%poloha vlny P je velmi ovlivnena tepovou frekvecni, proto je potreba najit
%pocet R vln. kter? odpovid? tepov? frekvence  v kazdem vstupnim signalu a podle toho nastavit pravidla
%definice integracnih okna, ve kterem se stanovuje vlna P. Pocet R vln je
%primo umerny tepove frekvence, cim je vetsi, tim vetsi TF.
delkaR = length(globR);
if delkaR >= 0 && delkaR <= 12
    konst1 = 120;
    konst2 = 10;
elseif delkaR >= 13 && delkaR <= 19
    konst1 = 120;
    konst2 = 0;
elseif delkaR >= 20 && delkaR <= 50
    konst1 = 100;
    konst2 = 0;
end

 vektor_P = [];
 for i=1:12
    pom_P1 = y_PT(i,:);
    pom_P2 = glob_zacatekQ;
    for j=2:length(pom_P2)
        pom_P3 = pom_P1(pom_P2(j)-konst1:pom_P2(j)-konst2);
        [pksP, locsP] = findpeaks(pom_P3); %nalezeni kladnych peaku
        [npksP, nlocsP] = findpeaks(-pom_P3); %nalezeni zapornych peaku
        if length(locsP)>=1 && length(nlocsP)>=1
            maximum1 = locsP(pksP==max(pksP)); %nalezeni nejvetsiho + peaku
            maximum2 = nlocsP(npksP==max(npksP)); %nalezeni nejvetsiho - peaku
            prah = npksP(npksP==max(npksP));
            okno_zacatek = maximum1; %definice polohy zacatku okna pro hledani vlny P
            if length(nlocsP)>=2
                npksP(npksP==max(npksP))=[];
                nlocsP(nlocsP==maximum2)=[];
                maximum3 = nlocsP(npksP==max(npksP)& npksP>(0.9*prah));%nalezeni druheho nejvetsiho - peaku
                if sum(maximum3)>0
                dist1 = abs(maximum1-maximum2);
                dist2 = abs(maximum1-maximum3);
                if dist1<=dist2
                    okno_konec = maximum2;
                else 
                    okno_konec = maximum3;
                end
                else
                    okno_konec = maximum2;
                end
            else
                okno_konec = maximum2;
            end
                
            if okno_zacatek>okno_konec
                pom_P4 = abs(pom_P3(okno_konec:okno_zacatek));
                pom_P5 = find(pom_P4==min(pom_P4));
                vlna_P(j) = pom_P2(j)-konst1+okno_konec-1+pom_P5-1;
            else
                pom_P4 = abs(pom_P3(okno_zacatek:okno_konec));
                pom_P5 = find(pom_P4==min(pom_P4));
                vlna_P(j) = pom_P2(j)-konst1+okno_zacatek-1+pom_P5-1;
            end
        end   
    end
            
   matice_PP{i,:} = vlna_P;
   vektor_P=[vektor_P matice_PP{i,:}/fvz];
 end
 glob_P = shlukovani(sort(vektor_P));
 
%-------------------nalezeni zacatku P vlny--------------------------------
%zavislost na TF
delkaR = length(globR);
if delkaR >= 0 && delkaR <= 16
    konst3 = 60;
elseif delkaR >= 17 && delkaR <= 20
    konst3 = 40;
elseif delkaR >= 21 && delkaR <= 50
    konst3 = 30;
end


vektor_zacatekP = [];
 for i=1:12
    pom_pb1 = y_PT(i,:);
    pom_pb2 = matice_PP{i,:};
   for j=2:length(pom_pb2)
       pom_pb3 = abs(pom_pb1(pom_pb2(j)-konst3:pom_pb2(j)));
       [pks_pb, locs_pb] = findpeaks(pom_pb3);
       if sum(pks_pb)==0
           locs_pb = 1;
           pks_pb = pom_pb3(1);
       else
           pks_pb = pks_pb;
           locs_pb = locs_pb;
       end
           posledni(j)=max(pks_pb);
           if posledni(j)<4.8 && delkaR <=11
               kpb = 3.5;%koeficient pro zacatky P vln
           elseif posledni(j)>11 && delkaR <=11
               kpb = 1.1;
           else
               kpb = 2;
           end
           Hpb(j) = max(pks_pb)/kpb;
           pom_pb4 = locs_pb(pks_pb==max(pks_pb));
           pom_pb5 = abs(pom_pb1((pom_pb2(j)-konst3):(pom_pb2(j)-konst3+pom_pb4-1)));
           pom_pb6 = find(pom_pb5 >= Hpb(j));
           P_z(j) = pom_pb2(j)-konst3+pom_pb6(1)-1;
   end
   matice_Hpb{i,:}= Hpb;
   P_zacatek{i,:}= P_z;
   
   vektor_zacatekP = [vektor_zacatekP P_zacatek{i,:}/fvz];
 end
 glob_zacatekP = shlukovani(sort(vektor_zacatekP));

 %-------------------nalezeni koncu P vlny---------------------------------
 %koeficient kpe pro nalezeni konce P vlny je adaptivni stredni hodnote
 %signalu v okne o delce 60 vzorku a zaroven je prizpusoben TF signalu
 %zkusebne hodnoty konstanty kpe se pohzbovali v rozmezi 1-3.5, stejnych
 %vysledku bylo dosazeno v pasmu 2.5-3.5, proto z celeho intervau byla
 %vybrana jen reprezentativni hodnota 3.5
 vektor_konecP = [];
 for i=1:12
    pom_pe1 = y_PT(i,:);
    pom_pe2 = matice_PP{i,:};
   for j=2:length(pom_pe2)
       pom_pe3 = abs(pom_pe1(pom_pe2(j):pom_pe2(j)+60));
       [pks_pe, locs_pe] = findpeaks(pom_pe3);
       prvni(j)=pks_pe(1);
       if prvni(j)<6 && delkaR>=10
           kpe=1.1;
       else
           kpe=3.5;
       end   
       Hpe(j) = pks_pe(1)/kpe;
       pom_pe4 = abs(pom_pe1(pom_pe2(j):pom_pe2(j)+locs_pe(1)-1+10));
       pom_pe5 = find(pom_pe4 >= Hpe(j));
       P_k(j) = pom_pe2(j)+pom_pe5(end)-1;
   end
   matice_Hpe{i,:}= Hpe;
   P_konec{i,:}= P_k;
   
   vektor_konecP=[vektor_konecP P_konec{i,:}/fvz];
 end
glob_konecP = shlukovani(sort(vektor_konecP));

%kontrola podminky-konec vlny P musi byt pred zacatkem vlny Q. pokud
%podminka neni splnena, pozice se prohodi
zelene =[];
cervene = [];
for i=1:12
    if length(glob_konecP)==length(glob_zacatekQ)
        for j=1:length(glob_zacatekQ)
            if glob_konecP(j)>glob_zacatekQ(j)
               zelene(j) = glob_zacatekQ(j);
               cervene(j) = glob_konecP(j);
            else
               zelene(j) = glob_konecP(j);
               cervene(j) = glob_zacatekQ(j);
            end
        end
    else
        zelene = glob_konecP;
        cervene = glob_zacatekQ;    
    end
end

%-------------------nalezeni vlny T----------------------------------------
%zavislost na TF
delkaR = length(globR);
if delkaR >=0 && delkaR <=9
    konst1 = 180;
    konst2 = 30;
elseif delkaR >=9 && delkaR <= 12
    konst1 = 140;
    konst2 = 20;
elseif delkaR >= 13 && delkaR <= 19
    konst1 = 120;
    konst2 = 20;
elseif delkaR >= 20 && delkaR <= 50
    konst1 = 100;
    konst2 = 0;
end


vektor_T = [];
 for i=1:12
    pom_T1 = y_PT(i,:);
    pom_T2 = glob_konecS;
    for j=1:length(pom_T2)
        pom_TS1 = pom_T1(pom_T2(j):pom_T2(j)+10);%zpresneni poloh hranic S vln
        pom_TS2 = find(pom_TS1==min(pom_TS1));
        S_presne(j) = pom_T2(j)+pom_TS2-1;
        pom_T3 = pom_T1(S_presne(j)+konst2:S_presne(j)+konst1);%definice okna
        [pksT, locsT] = findpeaks(pom_T3); %nalezeni kladnych peaku
        [npksT, nlocsT] = findpeaks(-pom_T3); %nalezeni zapornych peaku
        if length(locsT)>=1 && length(nlocsT)>=1
            maximum4 = locsT(pksT==max(pksT)); %nalezeni nejvetsiho + peaku
            maximum5 = nlocsT(npksT==max(npksT)); %nalezeni nejvetsiho - peaku
            okno_zacatekT = maximum4; %definice polohy zacatku okna pro hledani vlny P
            %prahT = npksT(npksT==max(npksT)); %definice prahu pro vyhledavani druheho maximalniho peaku
            if length(nlocsT)>=2
                npksT(npksT==max(npksT))=[];
                nlocsT(nlocsT==maximum5)=[];
                maximum6 = nlocsT(npksT==max(npksT));%nalezeni druheho nejvetsiho - peaku
                if sum(maximum6)>0 && maximum6>maximum5
                    okno_konecT= maximum6;
                else
                    okno_konecT = maximum5;
                end
            else
                okno_konecT = maximum5;
            end
            
            if okno_zacatekT > okno_konecT
                pom_T4 = abs(pom_T3(okno_konecT:okno_zacatekT));
                pom_T5 = find(pom_T4==min(pom_T4));
                vlna_T(j) = S_presne(j)+konst2+okno_konecT-1+pom_T5-1;
            else
                pom_T4 = abs(pom_T3(okno_zacatekT:okno_konecT));
                pom_T5 = find(pom_T4==min(pom_T4));
                vlna_T(j) = S_presne(j)+konst2+okno_zacatekT-1+pom_T5-1;
            end
        end   
    end
   
   matice_Spresne{i,:} = S_presne; 
   matice_TT{i,:} = vlna_T;
   vektor_T=[vektor_T matice_TT{i,:}/fvz];
 end
 glob_T = shlukovani(sort(vektor_T));
            
 %-------------------nalezeni koncu T vlny---------------------------------
 %zavislost na TF
 if delkaR>=13 && delkaR<20
    okno1 = 60;
    okno2 = 15;      
 elseif delkaR>=20
    okno1 = 30;
    okno2 = 5;
 else
    okno1 = 60;
    okno2 = 30;
 end
 
vektor_konecT = [];
 for i=1:12
    pom_te1 = y_PT(i,:);
    pom_te2 = matice_TT{i,:};
   for j=1:length(pom_te2)
       pom_te3 = abs(pom_te1(pom_te2(j):pom_te2(j)+okno1));
       [pks_te, locs_te] = findpeaks(pom_te3);
       hodnotaT(j)=pks_te(pks_te==max(pks_te));
       if hodnotaT(j)<6 && delkaR>=10
           kte=1.1;
       else
           kte=5;
       end   
       Hte(j) = pks_te(1)/kte;
       pom_te4 = abs(pom_te1(pom_te2(j):pom_te2(j)+locs_te(pks_te==max(pks_te))-1+okno2));
       pom_te5 = find(pom_te4 >= Hte(j));
       T_k(j) = pom_te2(j)+pom_te5(end)-1;
   end
   matice_Hte{i,:}= Hte;
   T_konec{i,:}= T_k;
   
   vektor_konecT=[vektor_konecT T_konec{i,:}/fvz];
 end
glob_konecT = shlukovani(sort(vektor_konecT)); 
glob_zacatekQ = glob_zacatekQ+10;
%---------------------globalni rozmereni EKG signalu ----------------------

figure
for i=1:3
    subplot(3,1,i)
    plot(time,x(i,:))
    xlabel('cas,[s]')
    ylabel('U,[µv]')
    hold on
    for j=2:length(cervene)
    line([cervene(j)/fvz;cervene(j)/fvz],[-1000;1500],'Color','r')
    end
    hold on
    for j=2:length(glob_konecS)
    line([glob_konecS(j)/fvz;glob_konecS(j)/fvz],[-1000;1500],'Color','r')
    end
    hold on
    for j=2:length(zelene)
    line([zelene(j)/fvz;zelene(j)/fvz],[-1000;1500],'Color','g')
    end
    hold on
    for j=2:length(glob_zacatekP)
    line([glob_zacatekP(j)/fvz;glob_zacatekP(j)/fvz],[-1000;1500],'Color','g')
    end
    hold on
    for j=2:length(glob_konecT)
    line([glob_konecT(j)/fvz;glob_konecT(j)/fvz],[-1000;1500],'Color','y')
    end
end


