clear all, clc

% výpoètové konstanty
a_a = 2;               % zrychlení [mm/s2]
a_d = 3;               % zpomalení [mm/s2]
dt = 0.001;             % výpoètový èasový krok
k = -100;               % konstanta, aby program poznal, že je konec matice a nehlásil chybu
v0 = 0;                 % rychlost na poèátku pohybu
s_prubeh = 0;           % poèáteèní poloha
time_celk = 0;          % výchozí èas
n = 1;                  % parametr urèující pož. øádek matice souøadnic
i = 1;                  % pøièítací index pro výsledné vektory
null = 1;               % konstanta pro zmìnu øádku matice
p = 0;                  % konstanta pro indikaci prvního nebo opakovaného prùchodu souøadnicí
 
% matice souøadnic A
w = 6;
%    X[mm] Y[mm] Z[mm]  v[mm/s]
A = [0     0     20     w
     30    50    20     w
     30    50    0      w   
     15    65    0      w
     0     50    0      w
     0     15    0      w
     15    0     0      w
     30    15    0      w
     30    15    20     w
     8     77    20     w
     8     77    0      w
     15    70    0      w
     22    77    0      w
     22    77    20     w
     37.5  65    20     w
     37.5  65    0      w
     52.5  0     0      w
     67.5  65    0      w
     67.5  65    20     w
     75    65    20     w
     75    65    0      w
     75    15    0      w
     90    0     0      w
     105   15    0      w
     105   65    0      w
     105   65    20     w
     112.5 65    20     w
     112.5 65    0      w
     142.5 65    0      w
     127.5 65    0      w
     127.5 0     0      w
     127.5 0     20     w
     0     0     20     w
     0     0     0      w   
     0     0     0      0
     0     0     0      0
     k     k     k      k];
 
%-----------------------------------------------------------------------
% smyèka pro propoèítání trajektorie
while n < (length(A)-3)%~



% -------< první výbìr z matice A >--------------------
 x0 = A(n,1);       % souradnice 
 y0 = A(n,2);
 z0 = A(n,3);
 x1 = A(n+1,1);
 y1 = A(n+1,2);
 z1 = A(n+1,3);
 
 v_p = A(n,4);      % rychlosti 
 %vk = A(n+1,4);
 
% ------< podmínka pro vybírání dalších øádkù matice A >-------- 
 if A(n+3,1) == k && A(n+3,2) == k && A(n+3,3) == k && A(n+3,4) == k
     x0 = A(n+1,1);
     y0 = A(n+1,2);
     z0 = A(n+1,3);
     x1 = A(n+1,1);
     y1 = A(n+1,2);
     z1 = A(n+1,3);
     v_p = A(n,4);      % rychlosti
     %vk = A(n+1,4);
 else           % když null = 0, tak se vybere další øádek matice
     if null == 0
         x0 = A(n+1,1);
         y0 = A(n+1,2);
         z0 = A(n+1,3);
         x1 = A(n+2,1);
         y1 = A(n+2,2);
         z1 = A(n+2,3);
         x2 = A(n+3,1);
         y2 = A(n+3,2);
         z2 = A(n+3,3);
         v_p = A(n+1,4);    % rychlosti
         %vk = A(n+2,4);
         n = n+1;           % pøièítání parametru n
     else
         x0 = A(n,1);
         y0 = A(n,2);
         z0 = A(n,3);
         x1 = A(n+1,1);
         y1 = A(n+1,2);
         z1 = A(n+1,3);
         x2 = A(n+2,1);
         y2 = A(n+2,2);
         z2 = A(n+2,3);
         v_p = A(n,4);
         %vk = A(n+1,4);
     end
 end


%----------------< interpolace jednoho profilu >-------------------------
%________________________________________________________________________
% výpoètové parametry
s = sqrt(power(x0-x1,2)+power(y0-y1,2)+power(z0-z1,2));

% úhel mezi vektory
u = [x1-x0 ; y1-y0; z1-z0];
v = [x2-x1 ; y2-y1; z2-z1];
u_length = sqrt(power(x0-x1,2)+power(y0-y1,2)+power(z0-z1,2));
v_length = sqrt(power(x2-x1,2)+power(y2-y1,2)+power(z2-z1,2));

fi=acos(((u(1)*v(1))+(u(2)*v(2))+(u(3)*v(3)))/(u_length*v_length));

% koncová rychlost jako funkce zmìny úhlu dráhy
if fi < (pi/2)
    %v_konc = v_p*(1-(fi*(2/pi)));                               %primka
    %v_konc = v_p*(1-(fi*(2/pi))^2)^(1/2);                          %elipsa
    v_konc = v_p*(1-(fi*(2/pi))^2);                             %parabola
    %v_konc = v_p*(exp(pi/2)-exp(fi))/(exp(fi)*(exp(pi/2)-1));    %%exponenciala
else
    v_konc = 0;
end
disp(['p = ',num2str(p),' [1]'])
if p == 1               % pokud je p = 1, tak cyklus probíhá znova
    v_konc = A(n,5);    % naèítání koncové souøadnice z matice A - pro opakované cykly
    if n == 1
        v0 = 0;
    else
    v0 = A(n-1,5);
    end
else
    A(n,5) = v_konc;    % ukládání koncové rychlosti do matice A
end
%p = 0;

disp(x0)
disp(x1)
disp([' '])
disp(['fi = ',num2str(fi*(180/pi)),' mm'])
disp(['u = ',num2str(u_length),' mm'])
disp(['v = ',num2str(v_length),' mm'])
disp(['n = ',num2str(n),' [1]'])
disp([' '])
disp(['v0 = ',num2str(v0),' mm/s'])
disp(['v_konc = ',num2str(v_konc),' mm/s'])
disp(['v_p = ',num2str(v_p),' mm/s'])
disp([' '])
%pause
if A(n+3,1) == k
    v_konc = 0;
end
%---------------------------------------------------------------------
% výpoèet èasù - dostateèná vzdálenost
t_a1 = abs((v_p-v0)/a_a);
t_da = abs((v_p-v0)/a_d);
t_d1 = (v_p-v_konc)/a_d;
t_ad = t_a1 + t_d1;

% dosažená rychlost pøi krátké dráze
v_traj = sqrt((s-power(v0,2)*((1/(2*a_a))-(1/a_a))+(power(v_konc,2)/(2*a_d)))/((1/(2*a_a))+(1/a_d)-(1/(2*a_d))));

% výpoèet èasù - kratší vzdálenost
t1 = (v_traj-v0)/a_a;
t11 = abs((v_traj-v0)/a_d);
t2 = (v_traj-v_konc)/a_d;

t12 = t1 + t2;


% podminka pro èas, pokud je poè. rychlost vìtší než požadovaná
if v0 > v_p
    t_a1 = t_da;
    t1 = t11;
end

% výbìr správného èasu zrychlení a zpomalení
if t12 < t_ad
    t_a = t1;
    t_d = t2;
    v_prom = v_traj;
else
    t_a = t_a1;
    t_d = t_d1;
    v_prom = v_p;
end

% dosahovaná rychlost pøi pøíliš krátké dráze
v_dos_a = v0 + a_a*t_a;
v_dos_d = v0 - a_d*t_d;
if v_dos_a < v_konc
    %t_a = (-v0 + sqrt(power(v0,2)+(2*a_a*s)))/(a_a);
    t_a = (2*s)/(v0+v_dos_a);
elseif v_dos_d > v_konc
    %t_d = (v0 - sqrt(power(v0,2)-(2*a_d*s)))/(a_d);
    t_d = (2*s)/(v0+v_dos_d);
end

% vypoèet dráhy pøi zrychlení, zpomalení i konstantní rychlosti
if v0 < v_p
s_a = 0.5*a_a*power(t_a,2)+v0*t_a;      
else
s_a = -0.5*a_a*power(t_a,2)+v0*t_a;         
end

s_d = v_prom*t_d - 0.5*a_d*power(t_d,2);   
s_v = s - s_a - s_d;
%A(n,5) = v_konc;

disp(['sa = ',num2str(s_a),' mm'])
disp(['sd = ',num2str(s_d),' mm'])
disp(['s = ',num2str(s),' mm'])
disp(['ta = ',num2str(t_a),' s'])

disp(['td = ',num2str(t_d),' s'])

disp(['A = [',num2str(A(n,1)),' ',num2str(A(n,2)),' ',num2str(A(n,3)),' ',num2str(A(n,4)),' ',num2str(A(n,5)),']'])
%pause
% podmínka pro pøíliš krátkou dráhu
if s <= s_a && v0 < v_konc
    disp('ZRYCHLOVANI');
    disp('Pozadovane rychlosti nelze na dane draze dosahnout');
    disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti');
    disp(' ');
    dv_max_a = a_a * t_a;
    v_konc = v0 + dv_max_a;
    A(n,5) = v_konc;
    p = 1;
    n = 1;
    null = 1;
    i = 1;
    v0 = 0;
    v_prubeh = 0;
    s_prubeh = 0;
    t_dop=0;
    time_celk = 0;
    continue
    %error('Pozadovane rychlosti nelze na dane draze dosahnout')
elseif s <= s_d && v0 > v_konc && s ~= 0
    disp('BRZDENI');
    disp('Pozadovane rychlosti nelze na dane draze dosahnout');
    disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti');
    disp(' ');
    dv_max_d = a_d * t_d;
    v0 = v_konc + dv_max_d;
    A(n-1,5) = v0;
    p = 1;
    n = 1;
    null = 1;
    i = 1;
    v0 = 0;
    v_prubeh = 0;
    s_prubeh = 0;
    t_dop=0;
    time_celk = 0;
    continue
    %error('Pozadovane rychlosti nelze na dane draze dosahnout')
end


% èas pohybu konstantni rychlostí
t_v = s_v / v_p;

% podmínka pro poèátek nebo konec drahy, kdy se rychlost nemìní
if t_d < 0
    t_d = 0;
elseif t_a < 0
    t_a = 0;
end
t_celk = t_a + t_v + t_d;

% poèáteèní hodnoty pro každou smyèku
v_prubeh(i) = v0;
%s_prubeh(i) = 0;
t_dop(i)=0;

%prùbìžné hodnoty
disp(['s = ',num2str(s),' mm'])
disp(['n = ',num2str(n),' [1]'])

disp(['v0 = ',num2str(v0),' mm/s'])
disp(['v_konc = ',num2str(v_konc),' mm/s'])
disp(['v_p = ',num2str(v_p),' mm/s'])
disp([' '])
disp(['t = ',num2str(time_celk(i)),' s'])
disp(['t_dop = ',num2str(t_dop(i)),' s'])
disp(['t_celk = ',num2str(t_celk),' s'])
disp([' '])


% smyèka výpoètu jednoho profilu
while t_dop(i) < t_celk
    
    % pøièítání èasu
    i = i+1;
    time_celk(i) = time_celk(i-1) + dt;
    t_dop(i) = t_dop(i-1)+dt;
    t_obr(i) = t_celk - t_dop(i);
    
    % výbìr zrychlení
    if t_dop(i) > t_a
        if t_obr(i) < 0
            a(i) = 0;
        else
            if t_obr(i) < t_d
                a(i) = -a_d;
            else
                a(i) = 0;
            end
        end
    else
        if v0 < v_p
            a(i) = a_a;
        else
            a(i) = -a_d;
        end
    end
    
    % numericka integrace
    v_prubeh(i) = v_prubeh(i-1)+a(i)*dt;
    s_prubeh(i) = s_prubeh(i-1) + 0.5*(v_prubeh(i)+v_prubeh(i-1))*dt;
    %m(i)=n;
    
end

% zapamatovani rychlosti na konci profilu
t_cc = length(v_prubeh);
v0 = v_prubeh(t_cc);


null = 0;

end

%------------------< VYKRESLENI >------------------------------------

t_c = length(time_celk);

% prùmìrné hodnoty rychlosti a zrychlení
v_avg = s_prubeh(t_c)/time_celk(t_c);
a_avg = v_prubeh(t_c)/time_celk(t_c);

disp(['Poloha v èase t, s = ',num2str(s_prubeh(t_c)),' mm'])
disp(['Prùmìrná rychlost, v_stø = ',num2str(v_avg),' mm/s'])
disp(['Pùmìrné zrychlení, a_stø = ',num2str(a_avg),' mm/s2'])

disp(['Èas t = ',num2str(time_celk(t_c)),' s'])

% -----------------< grafy >-----------------------------------
% tloustka èas v grafech
w = 2;

% vykresleni
figure(1)
subplot(3,1,1);plot(time_celk,a, 'k','linewidth',w)
hold on
plot([0 time_celk(t_c)],[a_avg a_avg],'r --')
grid on
title('Prùbìh zrychlení')
xlabel('t [s]')
ylabel('a [mm/s2]')
% xlim([0  t_c*dt]);
ylim([-1.5*abs(min(a))  1.5*max(a)]);
hold off

% prubeh rychlosti
subplot(3,1,2);plot(time_celk,v_prubeh,'k','linewidth',w)
hold on
plot([0 time_celk(t_c)],[v_avg v_avg],'r --')
grid on
title('Prùbìh rychlosti')
xlabel('t [s]')
ylabel('v [mm/s]')
% xlim([0  t_c*dt]);
ylim([-1.5*v0  1.5*max(v_prubeh)]);
hold off

% prubeh polohy
subplot(3,1,3);plot(time_celk,s_prubeh,'k','linewidth',w)
hold on
grid on
title('Prùbìh souøadnice polohy')
xlabel('t [s]')
ylabel('s [mm]')
%xlim([0  t_c*dt]);
ylim([-1.5*x0  1.5*max(s_prubeh)]);



% prubeh rychlosti
figure(2)
plot(time_celk,v_prubeh,'k','linewidth',w)
hold on
plot([0 time_celk(t_c)],[v_avg v_avg],'r --')
grid on
title('Prùbìh rychlosti')
xlabel('t [s]')
ylabel('v [mm/s]')
legend('Prùbìh rychlosti','Prùmìrná rychlost')
% xlim([0  t_c*dt]);
ylim([-1.5*v0  1.5*max(v_prubeh)]);
hold off
