%slouceni NC programu pro WeldPrint&Comau
%Jan Smaus

clear all
close all
clc

%definice promennych v programu
fi_A1 = pi/4; %uhel natoceni osy A1
L_n = 144; %delka nastrojove sestavy merena od cela vretene

%nacteni NC kodu
NC_obrob = load('Lopatka_obrabeni.mpf');
NC_podper = load('Lopatka_podepirani.mpf');

%
%% Usporadani souradnic do vektoru

NC_obrob_X = NC_obrob(:,2);
NC_obrob_Y = NC_obrob(:,3);
NC_obrob_Z = NC_obrob(:,4);

NC_podper_X = NC_podper(:,2);
NC_podper_Y = NC_podper(:,3);
NC_podper_Z = NC_podper(:,4);
NC_podper_A = NC_podper(:,5);
NC_podper_B = NC_podper(:,6);
NC_podper_C = NC_podper(:,7);

%
%% Transformace souradnic pro obrabeni

r_A1 = 88; %polomer otaceni souradneho systemu (osa A1 stroje)
Sour_obrabNC = [NC_obrob_Y, NC_obrob_Z]';
Sour_trans_obrob = [NC_obrob_Y-(sin(fi_A1)*r_A1), NC_obrob_Z-(r_A1-(cos(fi_A1)*r_A1))];
NC_pohon_Y = Sour_trans_obrob(:,1);
NC_pohon_Z = Sour_trans_obrob(:,2)+L_n;

%
%% Rozsireni velikosti vektoru souradnic podpery

M_obrob = length(NC_obrob_X);
M_podper = length(NC_podper_X);
N_nasobek =floor(M_obrob/M_podper);

NC_podper_L = zeros(M_obrob);
NC_podper_X_L = NC_podper_L(:,1);
NC_podper_Y_L = NC_podper_L(:,1);
NC_podper_Z_L = NC_podper_L(:,1);
NC_podper_A_L = NC_podper_L(:,1);
NC_podper_B_L = NC_podper_L(:,1);
NC_podper_C_L = NC_podper_L(:,1);

for i = 1:M_podper
    NC_podper_X_L(N_nasobek*(i-1)+1)=NC_podper_X(i);
end;
for i = 1:M_podper
    NC_podper_Y_L(N_nasobek*(i-1)+1)=NC_podper_Y(i);
end;
for i = 1:M_podper
    NC_podper_Z_L(N_nasobek*(i-1)+1)=NC_podper_Z(i);
end;
for i = 1:M_podper
    NC_podper_A_L(N_nasobek*(i-1)+1)=NC_podper_A(i);
end;
for i = 1:M_podper
    NC_podper_B_L(N_nasobek*(i-1)+1)=NC_podper_B(i);
end;
for i = 1:M_podper
    NC_podper_C_L(N_nasobek*(i-1)+1)=NC_podper_C(i);
end;

%
%% Zaplneni prazdnych pozici vektoru souradnic podpery

for i = 1:M_obrob
    if NC_podper_A_L(i) == 0
        NC_podper_A_L(i)=NC_podper_A_L(i-1);
    else
        NC_podper_A_L(i)=NC_podper_A_L(i);
    end
end

for i = 2:M_obrob
    if NC_podper_B_L(i) == 0
        NC_podper_B_L(i)=NC_podper_B_L(i-1);
    else
        NC_podper_B_L(i)=NC_podper_B_L(i);
    end
end

for i = 1:M_obrob
    if NC_podper_C_L(i) == 0
        NC_podper_C_L(i)=NC_podper_C_L(i-1);
    else
        NC_podper_C_L(i)=NC_podper_C_L(i);
    end
end

%
%% Prepocet absolutnich souradnic obrabeni na inkrementalni souradnice

NC_pohon_Y_inkrem = NC_podper_L(:,1);
NC_pohon_X_inkrem = NC_podper_L(:,1);

for i = 2:M_obrob
    NC_pohon_Y_inkrem(i) = NC_pohon_Y(i)-NC_pohon_Y(i-1);
end

for i = 2:M_obrob
    NC_pohon_X_inkrem(i) = NC_obrob_X(i)-NC_obrob_X(i-1);
end

%
%% Uprava souradnic podpery vzhledem k pohybu stolu od obrabeni

NC_podper_X_fin = NC_podper_X_L;
NC_podper_Y_fin = NC_podper_Y_L;
NC_podper_Z_fin = NC_podper_Z_L;
inkrem_Y_r = NC_pohon_Y_inkrem * cos(fi_A1);
inkrem_Z_r = NC_pohon_Y_inkrem * sin(fi_A1);

NC_podper_Y_fin(1) = NC_podper_Y_L(1)-(NC_pohon_Y(1)*cos(fi_A1));
NC_podper_Z_fin(1) = NC_podper_Z_L(1)+(NC_pohon_Y(1)*sin(fi_A1));

for i = 2:M_obrob
    if NC_podper_X_L(i) == 0
        NC_podper_X_fin(i) = -NC_obrob_X(i);
    else
        NC_podper_X_fin(i) = NC_podper_X_fin(i)-NC_obrob_X(i);
    end
end

for i = 2:M_obrob
    if NC_podper_Y_fin(i) == 0
        NC_podper_Y_fin(i) = NC_podper_Y_fin(i-1)-inkrem_Y_r(i);
    else
        NC_podper_Y_fin(i) = (NC_podper_Y_fin(i)-(NC_pohon_Y(i)*cos(fi_A1)))-inkrem_Y_r(i);
    end
end

for i = 2:M_obrob
    if NC_podper_Z_fin(i) == 0
        NC_podper_Z_fin(i) = NC_podper_Z_fin(i-1)+inkrem_Z_r(i);
    else
        NC_podper_Z_fin(i) = (NC_podper_Z_fin(i)+(NC_pohon_Y(i)*sin(fi_A1)))+inkrem_Z_r(i);
    end
end

%
%% Naplneni matice NC programu pro jednokanalove rizeni

NC_slouc = [NC_podper_X_fin, NC_podper_Y_fin, NC_podper_Z_fin, NC_podper_A_L, NC_podper_B_L, NC_podper_C_L, NC_obrob_X, NC_pohon_Y, NC_pohon_Z,];
fileID = fopen('NC_slouceni.mpf','w');
for j = 1:length(NC_slouc)
    fprintf(fileID,'X=%.4f Y=%.4f Z=%.4f A=%.4f B=%.4f C=%.4f X1=%.4f Y1=%.4f Z1=%.4f\n',NC_slouc(j,:));
end
fclose(fileID);