%% Vyhodnocení přesnosti a opakovatelnosti polohování

clc; clearvars ; close all; clf;

souradnice_pro_robot=xlsread("souradnice_pro_robot_excel_07_06.xlsx", 'A3:C32');
programovane_body=souradnice_pro_robot(:, 1:3);

zpusob_mereni=1; %volba průchodu bodů

namerene_body_s=load("namerene_body_02_03.mat");
namerene_body_c = struct2cell(namerene_body_s);
namerene_body = cell2mat(namerene_body_c);

N=30;%počet naměřených bodů - stejný pro oba způsoby průchodu bodů, je roven pětinásobku počtu cyklů
C=5;%počet cyklů

%

%PRESNOST    

if zpusob_mereni==1

body_P5_j = namerene_body(1:5:N, :);
body_P4_j = namerene_body(2:5:N, :);
body_P3_j = namerene_body(3:5:N, :);
body_P2_j = namerene_body(4:5:N, :);
body_P1_j = namerene_body(5:5:N, :);

body_P5_j_x=ones(C,1);
body_P5_j_y=ones(C,1);
body_P5_j_z=ones(C,1); 

body_P4_j_x=ones(C,1);
body_P4_j_y=ones(C,1);
body_P4_j_z=ones(C,1);

body_P3_j_x=ones(C,1);
body_P3_j_y=ones(C,1);
body_P3_j_z=ones(C,1);

body_P2_j_x=ones(C,1);
body_P2_j_y=ones(C,1);
body_P2_j_z=ones(C,1);

body_P1_j_x=ones(C,1);
body_P1_j_y=ones(C,1);
body_P1_j_z=ones(C,1);

for i=1:C
    body_P5_j_x(i) = body_P5_j(i,1);
    body_P5_j_y(i) = body_P5_j(i,2);
    body_P5_j_z(i) = body_P5_j(i,3);

    body_P4_j_x(i) = body_P4_j(i,1);
    body_P4_j_y(i) = body_P4_j(i,2);
    body_P4_j_z(i) = body_P4_j(i,3);

    body_P3_j_x(i) = body_P3_j(i,1);
    body_P3_j_y(i) = body_P3_j(i,2);
    body_P3_j_z(i) = body_P3_j(i,3);

    body_P2_j_x(i) = body_P2_j(i,1);
    body_P2_j_y(i) = body_P2_j(i,2);
    body_P2_j_z(i) = body_P2_j(i,3);

    body_P1_j_x(i) = body_P1_j(i,1);
    body_P1_j_y(i) = body_P1_j(i,2);
    body_P1_j_z(i) = body_P1_j(i,3);
end

body_P1_j_x_prumer = sum(body_P1_j_x)/C;
body_P1_j_y_prumer = sum(body_P1_j_y)/C;
body_P1_j_z_prumer = sum(body_P1_j_z)/C;

body_P2_j_x_prumer = sum(body_P2_j_x)/C;
body_P2_j_y_prumer = sum(body_P2_j_y)/C;
body_P2_j_z_prumer = sum(body_P2_j_z)/C;
 
body_P3_j_x_prumer = sum(body_P3_j_x)/C;
body_P3_j_y_prumer = sum(body_P3_j_y)/C;
body_P3_j_z_prumer = sum(body_P3_j_z)/C;

body_P4_j_x_prumer = sum(body_P4_j_x)/C;
body_P4_j_y_prumer = sum(body_P4_j_y)/C;
body_P4_j_z_prumer = sum(body_P4_j_z)/C;

body_P5_j_x_prumer = sum(body_P5_j_x)/C;
body_P5_j_y_prumer = sum(body_P5_j_y)/C;
body_P5_j_z_prumer = sum(body_P5_j_z)/C;

    bod_P5_c = programovane_body(1,:);
    bod_P4_c = programovane_body(2,:);
    bod_P3_c = programovane_body(3,:);
    bod_P2_c = programovane_body(4,:);
    bod_P1_c = programovane_body(5,:);

    AP_P_P1 = sqrt((body_P1_j_x_prumer-bod_P1_c(:,1))^2+(body_P1_j_y_prumer-bod_P1_c(:,2))^2+(body_P1_j_z_prumer-bod_P1_c(:,3))^2);
    AP_P_P2 = sqrt((body_P2_j_x_prumer-bod_P2_c(:,1))^2+(body_P2_j_y_prumer-bod_P2_c(:,2))^2+(body_P2_j_z_prumer-bod_P2_c(:,3))^2);
    AP_P_P3 = sqrt((body_P3_j_x_prumer-bod_P3_c(:,1))^2+(body_P3_j_y_prumer-bod_P3_c(:,2))^2+(body_P3_j_z_prumer-bod_P3_c(:,3))^2);
    AP_P_P4 = sqrt((body_P4_j_x_prumer-bod_P4_c(:,1))^2+(body_P4_j_y_prumer-bod_P4_c(:,2))^2+(body_P4_j_z_prumer-bod_P4_c(:,3))^2);
    AP_P_P5 = sqrt((body_P5_j_x_prumer-bod_P5_c(:,1))^2+(body_P5_j_y_prumer-bod_P5_c(:,2))^2+(body_P5_j_z_prumer-bod_P5_c(:,3))^2);

    
%OPAKOVATELNOST

l_P1 = ones(C,1);
l_P2 = ones(C,1);
l_P3 = ones(C,1);
l_P4 = ones(C,1);
l_P5 = ones(C,1);

for i=1:C

    l_P1(i) = sqrt((body_P1_j_x(i)-body_P1_j_x_prumer)^2+(body_P1_j_y(i)-body_P1_j_y_prumer)^2+(body_P1_j_z(i)-body_P1_j_z_prumer)^2);
    l_P2(i) = sqrt((body_P2_j_x(i)-body_P2_j_x_prumer)^2+(body_P2_j_y(i)-body_P2_j_y_prumer)^2+(body_P2_j_z(i)-body_P2_j_z_prumer)^2);
    l_P3(i) = sqrt((body_P3_j_x(i)-body_P3_j_x_prumer)^2+(body_P3_j_y(i)-body_P3_j_y_prumer)^2+(body_P3_j_z(i)-body_P3_j_z_prumer)^2);
    l_P4(i) = sqrt((body_P4_j_x(i)-body_P4_j_x_prumer)^2+(body_P4_j_y(i)-body_P4_j_y_prumer)^2+(body_P4_j_z(i)-body_P4_j_z_prumer)^2);
    l_P5(i) = sqrt((body_P5_j_x(i)-body_P5_j_x_prumer)^2+(body_P5_j_y(i)-body_P5_j_y_prumer)^2+(body_P5_j_z(i)-body_P5_j_z_prumer)^2);

end

l_P1_prumer = sum(l_P1)/C;
l_P2_prumer = sum(l_P2)/C;
l_P3_prumer = sum(l_P3)/C;
l_P4_prumer = sum(l_P4)/C;
l_P5_prumer = sum(l_P5)/C;

dev_P1_root=ones(C,1);
dev_P2_root=ones(C,1);
dev_P3_root=ones(C,1);
dev_P4_root=ones(C,1);
dev_P5_root=ones(C,1);

for i = 1:C

    dev_P1_root(i) = (l_P1(i)-l_P1_prumer)^2;
    dev_P2_root(i) = (l_P2(i)-l_P2_prumer)^2;
    dev_P3_root(i) = (l_P3(i)-l_P3_prumer)^2;
    dev_P4_root(i) = (l_P4(i)-l_P4_prumer)^2;
    dev_P5_root(i) = (l_P5(i)-l_P5_prumer)^2;

end

citatel_P1 = sum(dev_P1_root);
citatel_P2 = sum(dev_P2_root);
citatel_P3 = sum(dev_P3_root);
citatel_P4 = sum(dev_P4_root);
citatel_P5 = sum(dev_P5_root);

S_l_P1 = sqrt(citatel_P1/(C-1));
S_l_P2 = sqrt(citatel_P2/(C-1));
S_l_P3 = sqrt(citatel_P3/(C-1));
S_l_P4 = sqrt(citatel_P4/(C-1));
S_l_P5 = sqrt(citatel_P5/(C-1));

RP_P1 = l_P1_prumer + 3*S_l_P1;
RP_P2 = l_P1_prumer + 3*S_l_P2;
RP_P3 = l_P1_prumer + 3*S_l_P3;
RP_P4 = l_P1_prumer + 3*S_l_P4;
RP_P5 = l_P1_prumer + 3*S_l_P5;

end



% if zpusob_mereni==2
% 
% body_P5_j = namerene_body(1:2:60, :);
% body_P4_j = namerene_body(62:2:121, :);
% body_P3_j = namerene_body(122:2:181, :);
% body_P2_j = namerene_body(182:2:241, :);
% body_P1_j = namerene_body(242:2:300, :);
% 
% body_P5_j_x=ones(30,1);
% body_P5_j_y=ones(30,1);
% body_P5_j_z=ones(30,1); 
% 
% body_P4_j_x=ones(30,1);
% body_P4_j_y=ones(30,1);
% body_P4_j_z=ones(30,1);
% 
% body_P3_j_x=ones(30,1);
% body_P3_j_y=ones(30,1);
% body_P3_j_z=ones(30,1);
% 
% body_P2_j_x=ones(30,1);
% body_P2_j_y=ones(30,1);
% body_P2_j_z=ones(30,1);
% 
% body_P1_j_x=ones(30,1);
% body_P1_j_y=ones(30,1);
% body_P1_j_z=ones(30,1);
% 
% for i=1:30
%     body_P5_j_x(i) = body_P5_j(i,1);
%     body_P5_j_y(i) = body_P5_j(i,2);
%     body_P5_j_z(i) = body_P5_j(i,3);
% 
%     body_P4_j_x(i) = body_P4_j(i,1);
%     body_P4_j_y(i) = body_P4_j(i,2);
%     body_P4_j_z(i) = body_P4_j(i,3);
% 
%     body_P3_j_x(i) = body_P3_j(i,1);
%     body_P3_j_y(i) = body_P3_j(i,2);
%     body_P3_j_z(i) = body_P3_j(i,3);
% 
%     body_P2_j_x(i) = body_P2_j(i,1);
%     body_P2_j_y(i) = body_P2_j(i,2);
%     body_P2_j_z(i) = body_P2_j(i,3);
% 
%     body_P1_j_x(i) = body_P1_j(i,1);
%     body_P1_j_y(i) = body_P1_j(i,2);
%     body_P1_j_z(i) = body_P1_j(i,3);
% end
% 
% body_P1_j_x_prumer = sum(body_P1_j_x)/30;
% body_P1_j_y_prumer = sum(body_P1_j_y)/30;
% body_P1_j_z_prumer = sum(body_P1_j_z)/30;
% 
% body_P2_j_x_prumer = sum(body_P2_j_x)/30;
% body_P2_j_y_prumer = sum(body_P2_j_y)/30;
% body_P2_j_z_prumer = sum(body_P2_j_z)/30;
%  
% body_P3_j_x_prumer = sum(body_P3_j_x)/30;
% body_P3_j_y_prumer = sum(body_P3_j_y)/30;
% body_P3_j_z_prumer = sum(body_P3_j_z)/30;
% 
% body_P4_j_x_prumer = sum(body_P4_j_x)/30;
% body_P4_j_y_prumer = sum(body_P4_j_y)/30;
% body_P4_j_z_prumer = sum(body_P4_j_z)/30;
% 
% body_P5_j_x_prumer = sum(body_P5_j_x)/30;
% body_P5_j_y_prumer = sum(body_P5_j_y)/30;
% body_P5_j_z_prumer = sum(body_P5_j_z)/30;
% 
%     bod_P5_c = programovane_body(2,:);
%     bod_P4_c = programovane_body(62,:);
%     bod_P3_c = programovane_body(122,:);
%     bod_P2_c = programovane_body(182,:);
%     bod_P1_c = programovane_body(242,:);
% 
%     AP_P_P1 = sqrt((body_P1_j_x_prumer-bod_P1_c(:,1))^2+(body_P1_j_y_prumer-bod_P1_c(:,2))^2+(body_P1_j_z_prumer-bod_P1_c(:,3))^2);
%     AP_P_P2 = sqrt((body_P2_j_x_prumer-bod_P2_c(:,1))^2+(body_P2_j_y_prumer-bod_P2_c(:,2))^2+(body_P2_j_z_prumer-bod_P2_c(:,3))^2);
%     AP_P_P3 = sqrt((body_P3_j_x_prumer-bod_P3_c(:,1))^2+(body_P3_j_y_prumer-bod_P3_c(:,2))^2+(body_P3_j_z_prumer-bod_P3_c(:,3))^2);
%     AP_P_P4 = sqrt((body_P4_j_x_prumer-bod_P4_c(:,1))^2+(body_P4_j_y_prumer-bod_P4_c(:,2))^2+(body_P4_j_z_prumer-bod_P4_c(:,3))^2);
%     AP_P_P5 = sqrt((body_P5_j_x_prumer-bod_P5_c(:,1))^2+(body_P5_j_y_prumer-bod_P5_c(:,2))^2+(body_P5_j_z_prumer-bod_P5_c(:,3))^2);
% end


%Vyhodnoceni presnosti

% AP_P = [AP_P_P1;AP_P_P2;AP_P_P3;AP_P_P4;AP_P_P5;]
% csvwrite("presnost_02_03.csv", AP_P);

%Vyhodnoceni opakovatelnosti

RP_P = [RP_P1;RP_P2;RP_P3;RP_P4;RP_P5];
csvwrite("opakovatelnost_02_03.csv", RP_P);


