 %% urceni spravnosti pozice
clc
clear all
close all
TB = 'Jdi blize ke kamere';
TD = 'Jdi dale od kamery';
TS = 'Spravna vzdalenost';
TC = 'Vzpaz pro zacatek cviku';
TK = 'Cvik konci';

cas = 0;
bodyDetector = vision.CascadeObjectDetector('UpperBody');
bodyDetector.MinSize = [100 100];
bodyDetector.MergeThreshold = 10;
detector = posenet.PoseEstimator;
I = zeros(256,192,3,'uint8');
pomoc = 0;
x= 0;
vysl = zeros(6,1,'uint8');
cvik = inputdlg('Zadejte cislo cviku, ktery chcete provest (1 nebo 2)', 'Cislo cviku', [1 40]);
cvik = cell2mat(cvik);
if isempty(cvik) == 0
 cvik = str2num(cvik);
else
    mydialog3
end   

switch cvik
    case 1
obj=VideoReader('cvik_1.mov');
    case 2
obj=VideoReader('cvik_2.mov');
TC = 'Ruce za hlavu';
    
    otherwise
obj=VideoReader('cvik_1.mov'); 
end
snimky=obj.NumberOfFrames;

vid  = {};
index = 0;

writer_obj = VideoWriter('moje_video');
writer_obj.FrameRate = 30;

stl = read(obj,1);
stl_edge = im2gray(stl);
stl_edge = edge(stl_edge, 'Canny', [0.05 0.1]);

% ORIZNUTI POSTAVY
[radky_stl, sloupce_stl] = find(stl_edge); %nalezeni vsech nenulovych pixelu
radek_top_stl = min(radky_stl(:)); % urceni pozice nejhornejsiho nenuloveho pixelu
radek_bot_stl = max(radky_stl(:)); % urceni pozice nejspodnejsiho nenuloveho pixelu
sloupec_left_stl = min(sloupce_stl(:)); % urceni pozice nenuloveho pixelu nejvic vlevo
sloupec_right_stl = max(sloupce_stl(:)); % urceni pozice nenuloveho pixelu nejvicvpravo
riz_stl = stl_edge(radek_top_stl:radek_bot_stl, sloupec_left_stl:sloupec_right_stl);%orezany obraz

O = read(obj,snimky);

vstup = 2;
switch vstup
    case 0
cam = ipcam('http://192.168.0.157:8081/video','admin','admin'); % mobil
    case 1
cam = ipcam('http://192.168.0.10:8081/video','admin','admin'); % ntb
    case 2
cam = webcam; %pro notebook
end

Iinresize1 = imresize(O,[256 nan]);
Itmp1 = Iinresize1(:,(size(Iinresize1,2)-192)/2:(size(Iinresize1,2)-192)/2+192-1,:);
Icrop1 = Itmp1(1:256,1:192,1:3);
heatmaps1 = detector.predict(Icrop1);
keypoints1 = detector.heatmaps2Keypoints(heatmaps1);
mapa_1 = detector.heatmaps2Keypoints(heatmaps1);

h = figure;
set(h,'CloseRequestFcn',@closeRequest)

while isvalid(h)
snimek = snapshot(cam);
snimek = imresize(snimek,[720 1080]);
snimek = flip(snimek,2); 
frame = rgb2gray(snimek);
frame = imadjust(frame);
frame_canny = edge(frame, 'Canny', [0.1 0.7]);




switch x
    case 0

[radky_frame, sloupce_frame] = find(frame_canny); %nalezeni vsech nenulovych pixelu
radek_top_frame = min(radky_frame(:)); % urceni pozice nejhornejsiho nenuloveho pixelu
radek_bot_frame = max(radky_frame(:)); % urceni pozice nejspodnejsiho nenuloveho pixelu
sloupec_left_frame = min(sloupce_frame(:)); % urceni pozice nenuloveho pixelu nejvic vlevo

sloupec_right_frame = max(sloupce_frame(:)); % urceni pozice nenuloveho pixelu nejvic vpravo

riz_frame = frame_canny(radek_top_frame:radek_bot_frame,sloupec_left_frame:sloupec_right_frame);% orezany obraz

try
bboxes = bodyDetector(snimek);
snimek = insertObjectAnnotation(snimek,'rectangle',bboxes,'Telo');
catch
end

if size(riz_frame,1) > size(riz_stl,1)+1 & bboxes > 0
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TD, 'FontSize', 60);
elseif size(riz_frame,1) < size(riz_stl,1)-10 & bboxes > 0
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TB, 'FontSize', 60);
end
if size(riz_frame,1) < size(riz_stl,1)+1 & size(riz_frame,1) > size(riz_stl,1)-10
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TS, 'FontSize', 60);
pomoc = pomoc + 1;
if pomoc > 20
clf
x=1;
end
end
imshow(snimek);
drawnow();


    case 1

I = snimek;
Iinresize = imresize(I,[256 nan]);
Itmp = Iinresize(:,(size(Iinresize,2)-192)/2:(size(Iinresize,2)-192)/2+192-1,:);
Icrop = Itmp(1:256,1:192,1:3);

heatmaps = detector.predict(Icrop);
keypoints = detector.heatmaps2Keypoints(heatmaps);

Iout = detector.visualizeKeyPoints(Icrop,keypoints);
if sum(keypoints) ~= 0
suma = sum(keypoints(:,3));
korelace = corr2(keypoints1(6:11,:,:), keypoints(6:11,:,:));

for j = 6:11
   vychozi_poloha = [keypoints1(j,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints1(j,2)*(size(snimek,2)/(2*detector.InputSize(2))), 20];
   snimek = insertShape(snimek,"FilledCircle", vychozi_poloha,"Color","blue");
end

if korelace <= 0.95
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TC, 'FontSize', 60);
end

imshow(snimek)
drawnow();

if korelace > 0.95
rozdil = keypoints(6:11,:,:)-keypoints1(6:11,:,:); % aktualni - pozadovana

for i = 6:11
if rozdil(i-5,1) < 5 && rozdil(i-5,1) > -5 && rozdil(i-5,2) < 5 && rozdil(i-5,2) > -5
    vysl(i-5) = 1;
    barva = 'green';
else
    vysl(i-5) = 0;
    barva = 'red';
end
souradnice = [keypoints(i,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints(i,2)*(size(snimek,2)/(2*detector.InputSize(2))), 20];
snimek = insertShape(snimek,"FilledCircle", souradnice,"Color",barva,"Opacity", 0.4);


if sum(sum(vysl)) == size(vysl,1)*size(vysl,2)
cas = cas +1;
if cas > 10
   clf;
   x = 2;
end

end
end
if cas > 1 && sum(sum(vysl)) < size(vysl,1)*size(vysl,2)
    cas = cas -2;
end


end
end
hold on;
imshow(snimek)
drawnow();

  case 2

 snimek = insertText(snimek,[size(snimek,1)/2 size(snimek,2)/2], TK, 'FontSize', 60);
 imshow(snimek)
 pause(3)
 break;
  
    otherwise
        x =0;

end

index = index +1;
vid{index} = snimek;

end
        button = questdlg('Prejete si ulozit dosavadni prubeh', 'Ulozeni', 'Ano', 'Ne','Ne');
        switch button
            case 'Ano'
                if floor(length(vid)/30) < 1
                   mydialog
                   return
                end

            jmeno = inputdlg('Jak chcete pojmenovat video (bez pripony)?', 'Jmeno videa', [1 40]);
            slozka = dir;
            delka = 0;
            stejne = 0;
            e = 1;
                nazvy = {length(slozka)};
                while e <= length(slozka)
                    if slozka(e).isdir == 0
                        delka = delka+1;
                        nazvy{delka} = slozka(e).name;
                        stejne = strcmp(append(jmeno,'.avi'),nazvy{delka});
                    end
                    if stejne == 1
                      tlacitko = questdlg('Jmeno jiz existuje, chcete prepsat soubor?','Prepsani', 'Ano', 'Ne','Ne');
                      switch tlacitko
                          case 'Ano'
                              return
                              
                          case 'Ne'
                           jmeno = inputdlg('Jak chcete pojmenovat video (bez pripony)?', 'Jmeno videa', [1 40]);
                      end
                      e = 1;
                      stejne = 0;
                    end
                    e = e+1;
                end
                
            writer_obj = VideoWriter(jmeno{1}); 
            open(writer_obj)
            for c = 1:length(vid)
               frame_cvik = im2frame(vid{c});
            for v = 1:floor(length(vid)/30)
               writeVideo(writer_obj, frame_cvik); 
            end
            end
            close(writer_obj);
            
            case 'Ne'
                delete(writer_obj)
            
        end
clear cam;
close all force;    
 %% urceni spravnosti pozice PRAVA RUKA
clc
clear all
close all
TB = 'Jdi blize ke kamere';
TD = 'Jdi dale od kamery';
TS = 'Spravna vzdalenost';
TC = 'Vzpaz pro zacatek cviku';
TK = 'Cvik konci';
barva = 'red';
bodyDetector = vision.CascadeObjectDetector('UpperBody');
bodyDetector.MinSize = [100 100];
bodyDetector.MergeThreshold = 10;
detector = posenet.PoseEstimator;
I = zeros(256,192,3,'uint8');
pomoc = 0;
x= 3;
cas = 0;

vid  = {};
index = 0;
writer_obj = VideoWriter('moje_video');
writer_obj.FrameRate = 30;

vysl = zeros(5,1,'uint8');

cvik = inputdlg('Zadejte cislo cviku, ktery chcete provest (1 nebo 2)', 'Cislo cviku', [1 40]);
cvik = cell2mat(cvik);
if isempty(cvik) == 0
 cvik = str2num(cvik);
else
    mydialog3
end   

switch cvik
    case 1
obj=VideoReader('cvik_1.mov');        
    case 2
obj=VideoReader('cvik_2.mov');
TC = 'Ruce za hlavu';    
    otherwise
obj=VideoReader('cvik_1.mov'); 
end
snimky=obj.NumberOfFrames;
vyska=obj.Height;
sirka=obj.Width;
stl = read(obj,1);
stl_edge = im2gray(stl);
stl_edge = edge(stl_edge, 'Canny', [0.05 0.1]);

[radky_stl, sloupce_stl] = find(stl_edge); %nalezeni vsech nenulovych pixelu
radek_top_stl = min(radky_stl(:)); % urceni pozice nejhornejsiho nenuloveho pixelu
radek_bot_stl = max(radky_stl(:)); % urceni pozice nejspodnejsiho nenuloveho pixelu
sloupec_left_stl = min(sloupce_stl(:)); % urceni pozice nenuloveho pixelu nejvic vlevo
sloupec_right_stl = max(sloupce_stl(:)); % urceni pozice nenuloveho pixelu nejvicvpravo

riz_stl = stl_edge(radek_top_stl:radek_bot_stl, sloupec_left_stl:sloupec_right_stl);%orezany obraz

O = read(obj,snimky);

vstup = 2;
switch vstup
    case 0
cam = ipcam('http://192.168.0.157:8081/video','admin','admin'); % mobil
    case 1
cam = ipcam('http://192.168.0.10:8081/video','admin','admin'); % ntb
    case 2
cam = webcam; %pro notebook
end

Iinresize1 = imresize(O,[256 nan]);
Itmp1 = Iinresize1(:,(size(Iinresize1,2)-192)/2:(size(Iinresize1,2)-192)/2+192-1,:);
Icrop1 = Itmp1(1:256,1:192,1:3);
heatmaps1 = detector.predict(Icrop1);
keypoints1 = detector.heatmaps2Keypoints(heatmaps1);
Iout1 = detector.visualizeKeyPoints(Icrop1,keypoints1);


h = figure;
set(h,'CloseRequestFcn',@closeRequest)


while isvalid(h)
snimek = snapshot(cam);
snimek = imresize(snimek,[720 1080]);
snimek = flip(snimek,2);
frame = rgb2gray(snimek);
frame = imadjust(frame);
frame_canny = edge(frame, 'Canny', [0.1 0.7]);

switch x
    case 0
[radky_frame, sloupce_frame] = find(frame_canny); %nalezeni vsech nenulovych pixelu
radek_top_frame = min(radky_frame(:)); % urceni pozice nejhornejsiho nenuloveho pixelu
radek_bot_frame = max(radky_frame(:)); % urceni pozice nejspodnejsiho nenuloveho pixelu
sloupec_left_frame = min(sloupce_frame(:)); % urceni pozice nenuloveho pixelu nejvic vlevo

sloupec_right_frame = max(sloupce_frame(:)); % urceni pozice nenuloveho pixelu nejvic vpravo

riz_frame = frame_canny(radek_top_frame:radek_bot_frame,sloupec_left_frame:sloupec_right_frame);% orezany obraz

try
bboxes = bodyDetector(snimek);
snimek = insertObjectAnnotation(snimek,'rectangle',bboxes,'Telo');
catch
end

if size(riz_frame,1) > size(riz_stl,1)+10 & bboxes > 0
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TD, 'FontSize', 60);
elseif size(riz_frame,1) < size(riz_stl,1)-10 & bboxes > 0
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TB, 'FontSize', 60);
end

if size(riz_frame,1) < size(riz_stl,1)+10 & size(riz_frame,1) > size(riz_stl,1)-10
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TS, 'FontSize', 60);
pomoc = pomoc + 1;
if pomoc > 20
clf
x=1;
end
end

imshow(snimek);
drawnow();


    case 1

I = snimek;
Iinresize = imresize(I,[256 nan]);
Itmp = Iinresize(:,(size(Iinresize,2)-192)/2:(size(Iinresize,2)-192)/2+192-1,:);
Icrop = Itmp(1:256,1:192,1:3);

heatmaps = detector.predict(Icrop);
keypoints = detector.heatmaps2Keypoints(heatmaps);

Iout = detector.visualizeKeyPoints(Icrop,keypoints);
if sum(keypoints) ~= 0
suma = sum(keypoints(:,3));
korelace = corr2(keypoints1(6:11,:,:), keypoints(6:11,:,:));

for j = 6:11 
   vychozi_poloha = [keypoints(j,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints(j,2)*(size(snimek,2)/(2*detector.InputSize(2))), 20];
   snimek = insertShape(snimek,"FilledCircle", vychozi_poloha,"Color","blue");
end
if korelace <= 0.97
snimek = insertText(snimek,[size(snimek,1)/2-100 size(snimek,2)/2], TC, 'FontSize', 60);
end

imshow(snimek)
drawnow();

if korelace > 0.97
for w = 7:2:11
   rozdil(w-6) = keypoints(w,2)-keypoints(w-1,2);
   if rozdil(w-6) > 5
   vychozi_poloha = [keypoints(w,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints(w-1,2)*(size(snimek,2)/(2*detector.InputSize(2)))-rozdil(w-6), 20]; 
   elseif rozdil(w-6) < -5
   vychozi_poloha = [keypoints(w,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints(w-1,2)*(size(snimek,2)/(2*detector.InputSize(2)))+rozdil(w-6), 20];     
   else 
       vychozi_poloha = [keypoints(w,1)*(size(snimek,1)/detector.InputSize(1))+detector.InputSize(1),keypoints(w-1,2)*(size(snimek,2)/(2*detector.InputSize(2))), 20];
   end
   if rozdil(w-6) < 5 && rozdil(w-6) > -5 
      barva = 'green';
      vysl(w-6) = 1;
   else
       barva = 'red';
       vysl(w-6) = 0;
   end
   
   snimek = insertShape(snimek,"FilledCircle", vychozi_poloha,"Color",barva);
    
end

if sum(vysl) == 3 
cas = cas +1;
if cas > 15
x = 2;
clf;
end
end

if cas > 1 && sum(vysl) < 3
    cas = cas-2;
end
end




end
 hold on;
 imshow(snimek)
 drawnow();


    case 2
 snimek = insertText(snimek,[size(snimek,1)/2 size(snimek,2)/2], TK, 'FontSize', 60);
 imshow(snimek)
 pause(3)
 break;
 
    otherwise
        x = 0;


end
index = index +1;
vid{index} = snimek;
end

        button = questdlg('Prejete si ulozit dosavadni prubeh', 'Ulozeni', 'Ano', 'Ne','Ne');
        switch button
            case 'Ano'
                if floor(length(vid)/30) < 1
                   mydialog
                   return
                end

            jmeno = inputdlg('Jak chcete pojmenovat video (bez pripony)?', 'Jmeno videa', [1 40]);
            slozka = dir;
            delka = 0;
            stejne = 0;
            e = 1;
                nazvy = {length(slozka)};
                while e <= length(slozka)
                    if slozka(e).isdir == 0
                        delka = delka+1;
                        nazvy{delka} = slozka(e).name;
                        stejne = strcmp(append(jmeno,'.avi'),nazvy{delka});
                    end
                    if stejne == 1
                      tlacitko = questdlg('Jmeno jiz existuje, chcete prepsat soubor?','Prepsani', 'Ano', 'Ne','Ne');
                      switch tlacitko
                          case 'Ano'
                              return
                              
                          case 'Ne'
                           jmeno = inputdlg('Jak chcete pojmenovat video (bez pripony)?', 'Jmeno videa', [1 40]);
                      end
                      e = 1;
                      stejne = 0;
                    end
                    e = e+1;
                end
                
            writer_obj = VideoWriter(jmeno{1}); 
            open(writer_obj)
            for c = 1:length(vid)
               frame_cvik = im2frame(vid{c});
            for v = 1:floor(length(vid)/30)
               writeVideo(writer_obj, frame_cvik); 
            end
            end
            close(writer_obj);
            
            case 'Ne'
                delete(writer_obj)
            
        end

clear cam;
close all force ;

%% TESTOVANI METOD
clc
clear all
close all
p = 4;
switch p
    case 0
        model = 'UpperBody';
        popis = 'Telo';
        detector = vision.CascadeObjectDetector(model);
    case 1
         model = 'Mouth';
         popis = 'Usta';
         detector = vision.CascadeObjectDetector(model);
    case 2
        model = 'EyePairBig';
        popis = 'Oci';
        detector = vision.CascadeObjectDetector(model);
    case 3
        model = 'EyePairSmall';
        popis = 'Oci';
        detector = vision.CascadeObjectDetector(model);
    case 4
        model = 'ProfileFace';
        popis = 'Oblicej';
        detector = vision.CascadeObjectDetector(model);
    case 5
        model = 'Nose';
        popis = 'Nos';
        detector = vision.CascadeObjectDetector(model);
    case 6
        detector = vision.PeopleDetector;
        popis = 'Clovek';
    case 7
        detector = peopleDetectorACF;
        popis = 'Clovek';
    otherwise
        model = 'UpperBody';
        popis = 'Telo';
        detector = vision.CascadeObjectDetector(model);
 end

vstup = 1;
switch vstup
    case 0
cam = ipcam('http://192.168.0.157:8081/video','admin','admin'); % mobil
    case 1
cam = ipcam('http://192.168.0.10:8081/video','admin','admin'); % ntb
    case 2
cam = webcam; %pro notebook
end

h = figure;
set(h,'CloseRequestFcn',@closeRequest)
cas = zeros(60,1);
i = 1;


while isvalid(h)
snimek = snapshot(cam);
tic
try
bboxes = detector(snimek);
snimek = insertObjectAnnotation(snimek,'rectangle',bboxes,popis);
catch
end
if bboxes > 0
    pause(100)
end
imshow(snimek)
cas(i) = toc;
i = i+1
if i > 60
    break;
end
end
prumer = mean(cas);


function closeRequest(hObject,event)
tlacitko = questdlg('Prejete si zavrit okno?', 'Zavrit okno', 'Ano', 'Ne','Ne');
switch tlacitko
    
    case 'Ano'
        delete(hObject);
        close all force;
        %clear all;
        %clear cam;

 
    case 'Ne'
        return
end
end

function mydialog
    set(0,'units','pixels');
    obrazovka = get(0,'ScreenSize');
    d = dialog ('Position',[(obrazovka(3)-250)/2 (obrazovka(4)-150)/2 250 150], 'Name', 'Upozorneni');

    txt = uicontrol('Parent',d,...
               'Style','text',...
               'Position',[20 80 210 40],...
               'String','Delka videa je kratsi nez 1 sekunda, nebylo mozne ho ulozit. Pro ukonceni stisknete zavrit.');

    btn = uicontrol('Parent',d,...
               'Position',[85 20 70 25],...
               'String','Zavrit',...
               'Callback','delete(gcf)');
end

function mydialog2
    set(0,'units','pixels');
    obrazovka = get(0,'ScreenSize');
    d = dialog ('Position',[(obrazovka(3)-250)/2 (obrazovka(4)-150)/2 250 150], 'Name', 'Chybne jmeno');

    txt = uicontrol('Parent',d,...
               'Style','text',...
               'Position',[20 80 210 40],...
               'String','Jmeno jiz existuje, zvolte jine');

    btn = uicontrol('Parent',d,...
               'Position',[85 20 70 25],...
               'String','Zavrit',...
               'Callback','return');
end

function mydialog3
    set(0,'units','pixels');
    obrazovka = get(0,'ScreenSize');
    d = dialog ('Position',[(obrazovka(3)-250)/2 (obrazovka(4)-150)/2 250 150], 'Name', 'Chybne jmeno');

    txt = uicontrol('Parent',d,...
               'Style','text',...
               'Position',[20 80 210 40],...
               'String','Nezadal jste cislo cviku, program se ukoncuje');

    btn = uicontrol('Parent',d,...
               'Position',[85 20 70 25],...
               'String','Zavrit',...
               'Callback','delete(gcf)');
end
