%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Z A D A N I   P A R A M E T R U %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
close all


robot.sirka_y = 12; %m
robot.delka_x = 25; %m
robot.vyska_z = 3; %m

robot.delkaNuzek = 1.6; %m

robot.prisavkySirka = 2.55;%m
robot.prisavkyDelka = 1.8; %m

% výška minimálního svislého posunu při konečném polohování desky
svisle = 0.1; % [m]před vodorovnym posunem vyjede efektor svisle nahoru o xx [m]
svisle2 = 0.1; %  [m]svisle před koncem

% Diskretizační krok při numerickém výpočtu
h = 0.01; % [s]...krok

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% V S T U P N I   D A T A %%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parametry stohu
stoh.poloha = [-6 -3.6; -6 -1.2; -6  1.2; -6  3.6;
               -3 -3.6; -3 -1.2; -3  1.2; -3  3.6;
                0 -3.6;  0 -1.2;  0  1.2;  0  3.6;
                3 -3.6;  3 -1.2;
                6 -3.6;  6 -1.2]; % polohy [x y]

stoh.pocet = numel(stoh.poloha(:,1)); % pocet stohu....PRO RANDOM POČTY, JINAK JE TO NÍŽE

% stoh.typ = [1; 2; 3; 4; 5; 6; 7; 2; 5; 6; 4; 1; 7; 5; 3; 2]; % typy desek v jednotlivých stozích, řazeno dle stoh.poloha
stoh.typ = randi([1 7],stoh.pocet, 1); % pro náhodné typy stohů

stoh.pocetdesek = [23;92;15;83;54;100;7;44;10;97;0;78;82;87;8;40]; % počty desek v jednotlivých stozích
% stoh.pocetdesek = randi([0 100],stoh.pocet,1); %pro náhodné počty desek ve stohu

% Bod A a B
poc = [-9; -3.6; 0.1];
set = [6; 3.6; 0.02];


% Maximální rychlosti a zrychlení
vmax = 1.5; % [m/s] pro x, zbytek se dopočítá
amax = 0.5; % [m/s^2] pro x, zbytek se dopočítá

% Počáteční podmínky
s0 = [poc(1); poc(2); poc(3)];
v0 = [0; 0; 0];
a0 = [0; 0; 0];

maxvyska = robot.vyska_z - 0.2;


%% VÝPOČET DŮLEŽITÝCH HODNOT
stoh.pocet = numel(stoh.poloha(:,1)); % pocet stohu
poc2 = [poc(1), poc(2), poc(3)+svisle]; % aby tam vždy bylo alespoň 10 cm svislé dráhy
set2 = [set(1), set(2), set(3)+svisle2]; % viz poc2


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Z O B R A Z E N Í   O B J E K T Ů %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Robot
SimulacniModel_robot(poc(1),poc(2),poc(3),0, robot.sirka_y, robot.delka_x, robot.vyska_z, robot.delkaNuzek, robot.prisavkySirka, robot.prisavkyDelka);

% Stohy
[stoh.kraj_x, stoh.vyska, stoh.sirka(1:stoh.pocet,1), stoh.delka(1:stoh.pocet,1)] = Matice_desky_fcn(stoh.poloha(:,1), stoh.poloha(:,2),stoh.pocetdesek ,stoh.typ);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% D R Á H A   XY %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nx = [poc2(1), set2(1)];
ny = [poc2(2), set2(2)];
nz = [poc2(3), set2(3)];

% SOUŘADNICE Y
m = 1; % stupeň křivky
p = polyfit(nx, ny, m);
b1 = p(1);
b0 = p(2);

% optimalizace
xt = linspace(poc(1), set(1), 10000);
z = b1*xt + b0;

C = [xt; ones(size(xt))]';
d = z;
A = [];
b = [];

Aeq = [poc(1) 1;
       set(1) 1];

beq = [poc(2);
       set(2)];

coefy = lsqlin(C,d,A,b,Aeq,beq);

fyt = @(x) coefy(1)*x + coefy(2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% T V O R B A   B E Z K O L I Z N Í C H   B O D Ů %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    bezpecnost = 0.3; % [m]
    for k = 1:stoh.pocet
        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,1))) < stoh.sirka(k)*1.3 %*1.3 počítat s mezerou mezi stohy
            bezkoliznibody(4*k-3,1) = stoh.kraj_x(k,1); %násobeno celou deskou, protože půl stoh, půl je na manipulatoru
            bezkoliznibody(4*k-3,2) = fyt(bezkoliznibody(4*k-3,1));
            bezkoliznibody(4*k-3,3) = stoh.vyska(k) + bezpecnost; % bezpečnost xx cm nad deskou                         
        end
   
        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,2))) < stoh.sirka(k)*1.3
            bezkoliznibody(4*k-2,1) = stoh.kraj_x(k,2); %násobeno celou deskou, protože půl stoh, půl je na manipulatoru
            bezkoliznibody(4*k-2,2) = fyt(bezkoliznibody(4*k-2,1));
            bezkoliznibody(4*k-2,3) = stoh.vyska(k) + bezpecnost; % bezpečnost xx cm nad deskou
        end

        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,3))) < stoh.sirka(k)*1.3
            bezkoliznibody(4*k-1,1) = stoh.kraj_x(k,3); %násobeno celou deskou, protože půl stoh, půl je na manipulatoru
            bezkoliznibody(4*k-1,2) = fyt(bezkoliznibody(4*k-1,1)); 
            bezkoliznibody(4*k-1,3) = stoh.vyska(k) + bezpecnost; % bezpečnost xx cm nad deskou
        end
   
        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,4))) < stoh.sirka(k)*1.3
            bezkoliznibody(4*k,1) = stoh.kraj_x(k,4); %násobeno celou deskou, protože půl stoh, půl je na manipulatoru
            bezkoliznibody(4*k,2) = fyt(bezkoliznibody(4*k,1));
            bezkoliznibody(4*k,3) = stoh.vyska(k) + bezpecnost; % bezpečnost xx cm nad deskou
        end        
    end

% Vymazání buněk, kde v cestě žádný stoh nepřekážel, a tak je hodnota buňky [0 0 0]
bezkoliznibody = sortrows(bezkoliznibody);
mezi = bezkoliznibody;
mn = 1;
for m = 1 : numel(mezi(:,1))
    if mezi(m,:) == [0 0 0]
        bezkoliznibody(mn,:) = [];
        mn = mn-1;
    end
    mn = mn + 1;
end
mezi = bezkoliznibody;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Ú P R A V A   B E Z K O L I Z N Í C H   B O D Ů %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Vysoký stoh na začátku
mezi = sortrows(mezi);
for k = 1:stoh.pocet
    if abs(poc(1)-stoh.poloha(k,1)) < stoh.delka(k)*1.5
        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,1))) < stoh.sirka(k)*1.5
            if stoh.vyska(k) > poc2(3)
                poc2(3) = stoh.vyska(k) + svisle;
            end
        elseif abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,2))) < stoh.sirka(k)*1.5
            if stoh.vyska(k) > poc2(3)
                poc2(3) = stoh.vyska(k) + svisle;
            end
        end
    end
end

% Vysoký stoh na konci
for k = 1:stoh.pocet
    if abs(set(1)-stoh.poloha(k,1)) < stoh.delka(k)*1.3
        if abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,1))) < stoh.sirka*1.3
            if stoh.vyska(k) > set2(3)
                set2(3) = stoh.vyska(k) + svisle;
            end
        elseif abs(stoh.poloha(k,2) - fyt(stoh.kraj_x(k,2))) < stoh.sirka*1.3
            if stoh.vyska(k) > set2(3)
                set2(3) = stoh.vyska(k) + svisle;
            end
        end
    end
end

% Nějaký stoh nadměrně vysoký oproti ostatním
for m = 1 : numel(mezi(:,1))
    if mezi(m,3) > mean(mezi(:,3))*1.5
        if abs(set(1) - mezi(m,1)) > abs(set(1)-poc(1))/2
            if mezi(m,3)*0.8 > poc2(3)
                poc2(3) = mezi(m,3)*0.8;
            end
            if mezi(m,3)*0.7 > set2(3)
                if abs( set(1) - mezi(m,1) ) < 1.5*stoh.delka 
                    set2(3) = mezi(m,3)*0.7;
                end
            end
        elseif abs(set(1) - mezi(m,1)) < abs(set(1)-poc(1))/2
            if mezi(m,3)*0.8 > set2(3)
                set2(3) = mezi(m,3)*0.8;
            end
            if mezi(m,3)*0.7 > poc2(3)
                if abs( poc(1) - mezi(m,1) ) < 1.5*stoh.delka 
                    poc2(3) = mezi(m,3)*0.7;
                end
            end
        end
    end
end

% Vymazání příliš nízkých bodů mezi
for m = 1 : numel(mezi(:,1))
    % pokud je tento nízký bod u počátku
    if mezi(m,3) < mean(mezi(:,3))
        if abs(poc2(1) - mezi(m,1)) < stoh.delka
            mezi(m,3) = poc2(3);
        else
        % pokud je tento nízký bod mezi ostatními vysokými
            for n = 1: numel(mezi(:,1))
                if abs( mezi(m,1)-mezi(n,1) ) < stoh.delka
                    if  mezi(m,3) < mezi(n,3)*0.7 %trochu menší být může
                        mezi(m,:) = [0 0 0];
                    end
                elseif abs( mezi(m,2)-mezi(n,2) ) < stoh.sirka
                    if  mezi(m,3) < mezi(n,3)*0.7 %trochu menší být může
                        mezi(m,:) = [0 0 0];
                    end
                end
            end
        end
    end
end

% Vymazání označených bodů z matice mezi
[mezi] = vymazanimezi(mezi);

% Dva blízké stohy s malým rozdílem výšky
for k = 1:3 %2x opakovat tento krok... některé body se v první iteraci vyhnou vymazání
    mezi = sortrows(mezi);
    for  i = 1:numel(mezi(:,1))-1
        if abs( mezi(i,1) - mezi(i+1,1) ) < 0.3*stoh.delka(1) % něco víc než 1/4 delky desky, pak už je zvyšování bodu zbytečné
            if abs( mezi(i,3) - mezi(i+1,3) ) > 0.05 && abs( mezi(i,3) - mezi(i+1,3) ) < 1 % rozdíl mezi výškami je větší než 5cm, ale menší než 1 m
                if mezi(i,3) < mezi(i+1,3)
                    mezi(i,3) = mezi(i+1,3);                 
                else
                    mezi(i+1,3) = mezi(i,3);
                end
            end
        end
    end
    
    mezi = sortrows(mezi);
    for i = 1 : numel(mezi(:,1))
        if abs( poc(1) - mezi(i,1)) < stoh.delka*1.5
            if poc2(3) > mezi(i,3)
                mezi(i,:) = [0 0 0];
            end
        end
    end
end

% Vymazání bodu, který je zároveň níže než počátek a konec
for i = 1 : numel(mezi(:,1))
    if mezi(i,3) < poc2(3)
        if mezi(i,3) < set2(3)
            mezi(i,:) = [0 0 0];
        end
    end
end

% Vymazání označených bodů z matice mezi
[mezi] = vymazanimezi(mezi);

mezif = mezi;
for i = 1:numel(mezif(:,1))
    if mezi(i,3) < mean(mezif(:,3))
        if abs( mezi(i,1) - poc(1) ) > abs( set(1) - poc(1) )/3
            mezi(i,3) = mean(mezif(:,3));
        elseif abs( mezi(i,1) - set(1) ) > abs( set(1) - poc(1) )/3
            mezi(i,3) = mean(mezif(:,3));
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Z Á K L A D N Í   P O L Y N O M Y %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% SOUŘADNICE Z
if isempty(stoh.poloha)
    nz = [poc(3), set(3)];
    m = 1; % stupeň křivky
    p = polyfit(nx, nz, m);
    b1 = p(1);
    b0 = p(2);
    fzt = @(x) c1*x + c0;
else
    % Aplikace bezkoliznich bodu do polynomu
        nx = [poc2(1), mezi(:,1)', set2(1)];
        ny = [poc2(2), mezi(:,2)', set2(2)];
        nz = [poc2(3), mezi(:,3)', set2(3)];
    
    % POLYNOM ST.2
        n2 = 2; % stupeň křivky
        p = polyfit(nx, nz, n2);
        c2 = p(1);
        c1 = p(2);
        c0 = p(3); 
    %   fz2 = @(x) c2*x^2 + c1*x + c0;
        
        % optimalizace
        xt = linspace(poc(1), set(1), 10000);
        z = c2*xt.^2 + c1*xt + c0;
        
        C = [xt.^2; xt; ones(size(xt))]';
        d = z;
        A = [];
        b = [];
        
        Aeq = [poc2(1)^2 poc2(1) 1;
               set2(1)^2 set2(1) 1];
        
        beq = [poc2(3);
               set2(3)];
        
        coef2 = lsqlin(C,d,A,b,Aeq,beq);
        
        fz2 = @(x) coef2(1)*x^2 + coef2(2)*x + coef2(3);
    
    
    
    % POLYNOM ST.3
        n3 = 3; % stupeň křivky
        p = polyfit(nx, nz, n3);
        c3 = p(1);
        c2 = p(2);
        c1 = p(3);
        c0 = p(4); 
        
        % optimalizace
        xt = linspace(poc(1), set(1), 10000);
        z = c3*xt.^3 + c2*xt.^2 + c1*xt + c0;
        
        C = [xt.^3; xt.^2; xt; ones(size(xt))]';
        d = z;
        A = [];
        b = [];
        
        Aeq = [poc(1)^3 poc(1)^2 poc(1) 1;
               set(1)^3 set(1)^2 set(1) 1];
        
        beq = [poc2(3);
               set2(3)];
        
        coef3 = lsqlin(C,d,A,b,Aeq,beq);
        
        fz3 = @(x) coef3(1)*x^3 + coef3(2)*x^2 + coef3(3)*x + coef3(4);
    
    
    % POLYNOM ST. 4
        n4 = 4; % stupeň křivky
        p = polyfit(nx, nz, n4);
        c4 = p(1);
        c3 = p(2);
        c2 = p(3);
        c1 = p(4);
        c0 = p(5); 
       
        % optimalizace
        xt = linspace(poc(1), set(1), 10000);
        z = c4*xt.^4 + c3*xt.^3 + c2*xt.^2 + c1*xt + c0;
        
        C = [xt.^4; xt.^3; xt.^2; xt; ones(size(xt))]';
        d = z;
        A = [];
        b = [];
        
        Aeq = [poc(1)^4 poc(1)^3 poc(1)^2 poc(1) 1;
               set(1)^4 set(1)^3 set(1)^2 set(1) 1];
        
        beq = [poc2(3);
               set2(3)];
        
        coef4 = lsqlin(C,d,A,b,Aeq,beq);
        
        fz4 = @(x) coef4(1)*x^4 + coef4(2)*x^3 + coef4(3)*x^2 + coef4(4)*x + coef4(5);
    
    
            
    % POLYNOM ST. 5
        n5 = 5; % stupeň křivky
        p = polyfit(nx, nz, n5);
        c5 = p(1);
        c4 = p(2);
        c3 = p(3);
        c2 = p(4);
        c1 = p(5);
        c0 = p(6); 
        
        % optimalizace
        xt = linspace(poc(1), set(1), 10000);
        z = c5*xt.^5 + c4*xt.^4 + c3*xt.^3 + c2*xt.^2 + c1*xt + c0;
        
        C = [xt.^5; xt.^4; xt.^3; xt.^2; xt; ones(size(xt))]';
        d = z;
        A = [];
        b = [];
        
        Aeq = [poc(1)^5 poc(1)^4 poc(1)^3 poc(1)^2 poc(1) 1;
               set(1)^5 set(1)^4 set(1)^3 set(1)^2 set(1) 1];
        
        beq = [poc2(3);
               set2(3)];
        
        coef5 = lsqlin(C,d,A,b,Aeq,beq);
        
        fz5 = @(x) coef5(1)*x^5 + coef5(2)*x^4 + coef5(3)*x^3 + coef5(4)*x^2 + coef5(5)*x + coef5(6);
   
end

%% T R A J E K T O R I E   Z Á V I S L Á   N A   Č A S E

X = [poc(1) poc(2) poc(3)]; % matice polohy
v = [0 0 0];
a = [0 0 0];
j = [0 0 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% SVISLÁ TRAJEKTORIE - začátek
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
as = amax;

vmaxs1 = vmax;
v0 = 0;
jmss = as^2/abs((vmaxs1-v0));

%% ZRYCHLOVÁNÍ

% ČÁST 1
jm = jmss;
a0 = 0;
v0 = 0;
s0 = poc(3);

Ts1 = 2*as/jm;


aj = @(jm,a0,t) a0 + jm*t;
vj = @(jm,a0,v0,t) v0 + a0*t + jm*t^2/2;
fzj = @(jm,a0,v0,s0,t) v0*t + a0*t^2/2 + jm*t^3/6 + s0;

for ti = 1:Ts1/2/h
    x = poc(1);
    y = poc(2);
    zs11 = fzj(jm,a0,v0,s0,ti*h);
    v(ti+1,3) = vj(jm,a0,v0,ti*h);
    a(ti+1,3) = aj(jm,a0,ti*h);
    j(ti+1,3) = jm;

    X(ti+1,:) = [x y zs11];
end
L = length(X);
Ls11 = length(X);


% ZRYCHLOVÁNÍ - část 2
jm = -jmss;
a0 = a(end,3);
v0 = v(end,3);
s0 = X(end,3);

for ti = 1:Ts1/2/h
    x = poc(1);
    y = poc(2);
    zs12 = fzj(jm,a0,v0,s0,ti*h);
    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs12];
end
L = length(X);
Ls12 = length(X);



    while abs( X(end,3) - X(1,3) ) > 1/2*abs( poc2(3) - poc(3) )
        vmaxs1 = 0.9*vmaxs1;
        X(2:end,:) = [];
        v(2:end,:) = [];
        a(2:end,:) = [];
        j(2:end,:) = [];
        jmss = as^2/abs((vmaxs1-v0));

        % ČÁST 1
        jm = jmss;
        a0 = 0;
        v0 = 0;
        s0 = poc(3);
        
        Ts1 = 2*as/jm;
        vh = vmax/2;
        

        for ti = 1:Ts1/2/h
            x = poc(1);
            y = poc(2);
            zs11 = fzj(jm,a0,v0,s0,ti*h);
            v(ti+1,3) = vj(jm,a0,v0,ti*h);
            a(ti+1,3) = aj(jm,a0,ti*h);
            j(ti+1,3) = jm;
        
            X(ti+1,:) = [x y zs11];
        end
        L = length(X);
        Ls11 = length(X);
        
        
        % ZRYCHLOVÁNÍ - část 2
        jm = -jmss;
        a0 = a(end,3);
        v0 = v(end,3);
        s0 = X(end,3);
        
        for ti = 1:Ts1/2/h
            x = poc(1);
            y = poc(2);
            zs12 = fzj(jm,a0,v0,s0,ti*h);
            v(ti+L,3) = vj(jm,a0,v0,ti*h);
            a(ti+L,3) = aj(jm,a0,ti*h);
            j(ti+L,3) = jm;
        
            X(ti+L,:) = [x y zs12];
        end
        L = length(X);
        Ls12 = length(X);
        
    end



%% KONSTANTNÍ RYCHLOST
jm = 0;
a0 = 0;
v0 = v(end,3);
s0 = X(end,3);

% tirk = abs(abs(5*s5)-2*abs(zs12-poc(3)))/(abs(v0));
t_lins1 = abs(poc2(3)-poc(3)-2*abs( X(end,3)-poc(3) ) ) / abs(v0);

for ti = 1:t_lins1/h % +1 aby to dojelo správně do koncového bodu
    x = poc(1);
    y = poc(2);
    zs13 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs13];

end
L = length(X);
Ls13 = length(X);

%% ZPOMALOVÁNÍ

% ČÁST 1
jm = -jmss;
a0 = 0;
v0 = v(end,3);
s0 = X(end,3);

for ti = 1 : Ts1/2/h
    x = poc(1);
    y = poc(2);
    zs14 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs14];

end
L = length(X);
Ls14 = length(X);


% ČÁST 2
% Concave period
jm = jmss;
a0 = a(end,3);
v0 = v(end,3);
s0 = X(end,3);

for ti = 1 : Ts1/2/h
    x = poc(1);
    y = poc(2);
    zs15 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs15];
end
L = length(X);
Ls15 = length(X);

time = 2*Ts1 + t_lins1;

j2 = j; j3 = j; j4 = j; j5 = j;
a2 = a; a3 = a; a4 = a; a5 = a;
v2 = v; v3 = v; v4 = v; v5 = v;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% F U N K C E   Z A V I S L E   N A   C A S E %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

as = amax;
v0 = 0;
jms = as^2/(vmax-v0);

% Funkce polohy
fxj = @(jm,a0,v0,s0,t) v0*t + a0*t^2/2 + jm*t^3/6 + s0;
fyj = @(jm,a0,v0,s0,t) coefy(1)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0) + coefy(2);

fzj2 = @(jm,a0,v0,s0,t) coef2(1)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^2  +  coef2(2)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)  +  coef2(3);
fzj3 = @(jm,a0,v0,s0,t) coef3(1)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^3  +  coef3(2)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^2  +  coef3(3)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)  +  coef3(4);
fzj4 = @(jm,a0,v0,s0,t) coef4(1)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^4  +  coef4(2)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^3  +  coef4(3)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^2  +  coef4(4)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)  +  coef4(5);
fzj5 = @(jm,a0,v0,s0,t) coef5(1)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^5  +  coef5(2)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^4  +  coef5(3)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^3  +  coef5(4)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)^2  +  coef5(5)*(v0*t + a0*t^2/2 + jm*t^3/6 + s0)  +  coef5(6);


% Funkce rychlosti
vxj = @(jm,a0,v0,t) v0 + a0*t + jm*t^2/2;
vyj = @(jm,a0,v0,t) coefy(1)*(v0 + a0*t + jm*t^2/2);

vzj2 = @(jm,a0,v0,t) coef2(1)*(v0 + a0*t + jm*t^2/2)^2  +  coef2(2)*(v0 + a0*t + jm*t^2/2);
vzj3 = @(jm,a0,v0,t) coef3(1)*(v0 + a0*t + jm*t^2/2)^3  +  coef3(2)*(v0 + a0*t + jm*t^2/2)^2  +  coef3(3)*(v0 + a0*t + jm*t^2/2);
vzj4 = @(jm,a0,v0,t) coef4(1)*(v0 + a0*t + jm*t^2/2)^4  +  coef4(2)*(v0 + a0*t + jm*t^2/2)^3  +  coef4(3)*(v0 + a0*t + jm*t^2/2)^2  +  coef4(4)*(v0 + a0*t + jm*t^2/2);
vzj5 = @(jm,a0,v0,t) coef5(1)*(v0 + a0*t + jm*t^2/2)^5  +  coef5(2)*(v0 + a0*t + jm*t^2/2)^4  +  coef5(3)*(v0 + a0*t + jm*t^2/2)^3  +  coef5(4)*(v0 + a0*t + jm*t^2/2)^2  +  coef5(5)*(v0 + a0*t + jm*t^2/2);

% Funkce zrychlení
axj = @(jm,a0,t) a0 + jm*t;
ayj = @(jm,a0,t) coefy(1)*(a0 + jm*t);

azj2 = @(jm,a0,t) coef2(1)*(a0 + jm*t)^2  +  coef2(2)*(a0 + jm*t);
azj3 = @(jm,a0,t) coef3(1)*(a0 + jm*t)^3  +  coef3(2)*(a0 + jm*t)^2  +  coef3(3)*(a0 + jm*t);
azj4 = @(jm,a0,t) coef4(1)*(a0 + jm*t)^4  +  coef4(2)*(a0 + jm*t)^3  +  coef4(3)*(a0 + jm*t)^2  +  coef4(4)*(a0 + jm*t);
azj5 = @(jm,a0,t) coef5(1)*(a0 + jm*t)^5  +  coef5(2)*(a0 + jm*t)^4  +  coef5(3)*(a0 + jm*t)^3  +  coef5(4)*(a0 + jm*t)^2  +  coef5(5)*(a0 + jm*t);


% Jerk
jx = @(jm) jm;
jy = @(jm) coefy(1)*jm;

jz2 = @(jm) coef2(1)*(jm)^2  +  coef2(2)*(jm);
jz3 = @(jm) coef3(1)*(jm)^3  +  coef3(2)*(jm)^2  +  coef3(3)*(jm);
jz4 = @(jm) coef4(1)*(jm)^4  +  coef4(2)*(jm)^3  +  coef4(3)*(jm)^2  +  coef4(4)*(jm);
jz5 = @(jm) coef5(1)*(jm)^5  +  coef5(2)*(jm)^4  +  coef5(3)*(jm)^3  +  coef5(4)*(jm)^2  +  coef5(5)*(jm);

X2 = X; X3 = X; X4 = X; X5 = X;

%% TRAJEKTORIE - ZRYCHLOVÁNÍ

%Convex period
jm = jms;
a0 = 0;
v0 = 0;
s0 = X2(end,1);

T = 2*as/jm;
vh = vmax/2;

for ti = 1 : T/2/h
    x = fxj(jm,a0,v0,s0,ti*h);
    y = fyj(jm,a0,v0,s0,ti*h);
    X2(ti+L,3) = fzj2(jm,a0,v0,s0,ti*h);
    X3(ti+L,3) = fzj3(jm,a0,v0,s0,ti*h);
    X4(ti+L,3) = fzj4(jm,a0,v0,s0,ti*h);
    X5(ti+L,3) = fzj5(jm,a0,v0,s0,ti*h);


    X2(ti+L,1:2) = [x y];
    X3(ti+L,1:2) = [x y];
    X4(ti+L,1:2) = [x y];
    X5(ti+L,1:2) = [x y];

    v(ti+L,1) = vxj(jm,a0,v0,ti*h);
    v(ti+L,2) = vyj(jm,a0,v0,ti*h);
    v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
    v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
    v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
    v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);

    a(ti+L,1) = axj(jm,a0,ti*h);
    a(ti+L,2) = ayj(jm,a0,ti*h);
    a2(ti+L,3) = azj2(jm,a0,ti*h);
    a3(ti+L,3) = azj3(jm,a0,ti*h);
    a4(ti+L,3) = azj4(jm,a0,ti*h);
    a5(ti+L,3) = azj5(jm,a0,ti*h);

    j(ti+L,1) = jx(jm);
    j(ti+L,2) = jy(jm);
    j2(ti+L,3) = jz2(jm);
    j3(ti+L,3) = jz3(jm);
    j4(ti+L,3) = jz4(jm);
    j5(ti+L,3) = jz5(jm);
end
L = length(X2);
L1 = length(X2);


% CONCAVE PERIOD (ČÁST 2)
jm = -jms;
a0 = as;
v0 = vh;
s0 = X2(end,1);

for ti = 1:T/2/h
    x = fxj(jm,a0,v0,s0,ti*h);
    y = fyj(jm,a0,v0,s0,ti*h);
    X2(ti+L,3) = fzj2(jm,a0,v0,s0,ti*h);
    X3(ti+L,3) = fzj3(jm,a0,v0,s0,ti*h);
    X4(ti+L,3) = fzj4(jm,a0,v0,s0,ti*h);
    X5(ti+L,3) = fzj5(jm,a0,v0,s0,ti*h);


    X2(ti+L,1:2) = [x y];
    X3(ti+L,1:2) = [x y];
    X4(ti+L,1:2) = [x y];
    X5(ti+L,1:2) = [x y];

    v(ti+L,1) = vxj(jm,a0,v0,ti*h);
    v(ti+L,2) = vyj(jm,a0,v0,ti*h);
    v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
    v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
    v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
    v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);

    a(ti+L,1) = axj(jm,a0,ti*h);
    a(ti+L,2) = ayj(jm,a0,ti*h);
    a2(ti+L,3) = azj2(jm,a0,ti*h);
    a3(ti+L,3) = azj3(jm,a0,ti*h);
    a4(ti+L,3) = azj4(jm,a0,ti*h);
    a5(ti+L,3) = azj5(jm,a0,ti*h);

    j(ti+L,1) = jx(jm);
    j(ti+L,2) = jy(jm);
    j2(ti+L,3) = jz2(jm);
    j3(ti+L,3) = jz3(jm);
    j4(ti+L,3) = jz4(jm);
    j5(ti+L,3) = jz5(jm);
end
L = length(X);
L2 = length(X2);


    while abs( X2(end,1) - X(1,1) ) > 1/2*abs( set(1) - poc(1) )
        vmax = 0.9*vmax;
        x = [];
        y = [];
        X2(Ls15+1:end,:) = [];
        X3(Ls15+1:end,:) = [];
        X4(Ls15+1:end,:) = [];
        X5(Ls15+1:end,:) = [];

        jms = as^2/(vmax-0);
        L = Ls15;

        % TRAJEKTORIE - ZRYCHLOVÁNÍ
        %Convex period
        jm = jms;
        a0 = 0;
        v0 = 0;
        s0 = X2(end,1);
        
        T = 2*as/jm;
        vh = vmax/2;
        

        for ti = 1 : T/2/h
            x = fxj(jm,a0,v0,s0,ti*h);
            y = fyj(jm,a0,v0,s0,ti*h);
            X2(ti+Ls15,3) = fzj2(jm,a0,v0,s0,ti*h);
            X3(ti+Ls15,3) = fzj3(jm,a0,v0,s0,ti*h);
            X4(ti+Ls15,3) = fzj4(jm,a0,v0,s0,ti*h);
            X5(ti+Ls15,3) = fzj5(jm,a0,v0,s0,ti*h);
        
        
            X2(ti+Ls15,1:2) = [x y];
            X3(ti+Ls15,1:2) = [x y];
            X4(ti+Ls15,1:2) = [x y];
            X5(ti+Ls15,1:2) = [x y];
        
            v(ti+L,1) = vxj(jm,a0,v0,ti*h);
            v(ti+L,2) = vyj(jm,a0,v0,ti*h);
            v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
            v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
            v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
            v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);
        
            a(ti+L,1) = axj(jm,a0,ti*h);
            a(ti+L,2) = ayj(jm,a0,ti*h);
            a2(ti+L,3) = azj2(jm,a0,ti*h);
            a3(ti+L,3) = azj3(jm,a0,ti*h);
            a4(ti+L,3) = azj4(jm,a0,ti*h);
            a5(ti+L,3) = azj5(jm,a0,ti*h);
        
            j(ti+L,1) = jx(jm);
            j(ti+L,2) = jy(jm);
            j2(ti+L,3) = jz2(jm);
            j3(ti+L,3) = jz3(jm);
            j4(ti+L,3) = jz4(jm);
            j5(ti+L,3) = jz5(jm);
        end
        
        L1 = length(X2);
        L = L1;
        
        % CONCAVE PERIOD (ČÁST 2)
        jm = -jms;
        a0 = as;
        v0 = vh;
        s0 = X2(end,1);

        for ti = 1:T/2/h
            x = fxj(jm,a0,v0,s0,ti*h);
            y = fyj(jm,a0,v0,s0,ti*h);
            X2(ti+L1,3) = fzj2(jm,a0,v0,s0,ti*h);
            X3(ti+L1,3) = fzj3(jm,a0,v0,s0,ti*h);
            X4(ti+L1,3) = fzj4(jm,a0,v0,s0,ti*h);
            X5(ti+L1,3) = fzj5(jm,a0,v0,s0,ti*h);
        
        
            X2(ti+L1,1:2) = [x y];
            X3(ti+L1,1:2) = [x y];
            X4(ti+L1,1:2) = [x y];
            X5(ti+L1,1:2) = [x y];
        
            v(ti+L,1) = vxj(jm,a0,v0,ti*h);
            v(ti+L,2) = vyj(jm,a0,v0,ti*h);
            v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
            v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
            v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
            v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);
        
            a(ti+L,1) = axj(jm,a0,ti*h);
            a(ti+L,2) = ayj(jm,a0,ti*h);
            a2(ti+L,3) = azj2(jm,a0,ti*h);
            a3(ti+L,3) = azj3(jm,a0,ti*h);
            a4(ti+L,3) = azj4(jm,a0,ti*h);
            a5(ti+L,3) = azj5(jm,a0,ti*h);
        
            j(ti+L,1) = jx(jm);
            j(ti+L,2) = jy(jm);
            j2(ti+L,3) = jz2(jm);
            j3(ti+L,3) = jz3(jm);
            j4(ti+L,3) = jz4(jm);
            j5(ti+L,3) = jz5(jm);
        end
        
        L2 = length(X2);

    end

L = length(X2);

%% TRAJEKTORIE - ČÁST S MAXIMÁLNÍ RYCHLOSTÍ
% Funkce závislé na čase
jm = 0;
a0 = 0;
v0 = vmax;
s0 = X2(end,1);

vlinf = @(t) v0;

% délka dráhy s linearní rychlostí (x dráha) + čas
s = abs(poc(1) - set(1)) - 2*abs(poc(1) - X2(end,1));
t_lin  = s/vmax;

for ti = 1 : t_lin/h
    x = fxj(jm,a0,v0,s0,ti*h);
    y = fyj(jm,a0,v0,s0,ti*h);
    X2(ti+L,3) = fzj2(jm,a0,v0,s0,ti*h);
    X3(ti+L,3) = fzj3(jm,a0,v0,s0,ti*h);
    X4(ti+L,3) = fzj4(jm,a0,v0,s0,ti*h);
    X5(ti+L,3) = fzj5(jm,a0,v0,s0,ti*h);


    X2(ti+L,1:2) = [x y];
    X3(ti+L,1:2) = [x y];
    X4(ti+L,1:2) = [x y];
    X5(ti+L,1:2) = [x y];

    v(ti+L,1) = vxj(jm,a0,v0,ti*h);
    v(ti+L,2) = vyj(jm,a0,v0,ti*h);
    v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
    v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
    v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
    v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);

    a(ti+L,1) = axj(jm,a0,ti*h);
    a(ti+L,2) = ayj(jm,a0,ti*h);
    a2(ti+L,3) = azj2(jm,a0,ti*h);
    a3(ti+L,3) = azj3(jm,a0,ti*h);
    a4(ti+L,3) = azj4(jm,a0,ti*h);
    a5(ti+L,3) = azj5(jm,a0,ti*h);

    j(ti+L,1) = jx(jm);
    j(ti+L,2) = jy(jm);
    j2(ti+L,3) = jz2(jm);
    j3(ti+L,3) = jz3(jm);
    j4(ti+L,3) = jz4(jm);
    j5(ti+L,3) = jz5(jm);
end
L = length(X2);
L3 = length(X2);

%% TRAJEKTORIE - ZPOMALOVÁNÍ
vh = vmax/2;
%Concave period
jm = -jms;
a0 = 0;
v0 = vmax;
s0 = X2(end,1);

for ti = 1 : T/2/h
    x = fxj(jm,a0,v0,s0,ti*h);
    y = fyj(jm,a0,v0,s0,ti*h);
    X2(ti+L,3) = fzj2(jm,a0,v0,s0,ti*h);
    X3(ti+L,3) = fzj3(jm,a0,v0,s0,ti*h);
    X4(ti+L,3) = fzj4(jm,a0,v0,s0,ti*h);
    X5(ti+L,3) = fzj5(jm,a0,v0,s0,ti*h);


    X2(ti+L,1:2) = [x y];
    X3(ti+L,1:2) = [x y];
    X4(ti+L,1:2) = [x y];
    X5(ti+L,1:2) = [x y];

    v(ti+L,1) = vxj(jm,a0,v0,ti*h);
    v(ti+L,2) = vyj(jm,a0,v0,ti*h);
    v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
    v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
    v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
    v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);

    a(ti+L,1) = axj(jm,a0,ti*h);
    a(ti+L,2) = ayj(jm,a0,ti*h);
    a2(ti+L,3) = azj2(jm,a0,ti*h);
    a3(ti+L,3) = azj3(jm,a0,ti*h);
    a4(ti+L,3) = azj4(jm,a0,ti*h);
    a5(ti+L,3) = azj5(jm,a0,ti*h);

    j(ti+L,1) = jx(jm);
    j(ti+L,2) = jy(jm);
    j2(ti+L,3) = jz2(jm);
    j3(ti+L,3) = jz3(jm);
    j4(ti+L,3) = jz4(jm);
    j5(ti+L,3) = jz5(jm);
end
L = length(X2);
L4 = length(X2);

% Concave period
jm = jms;
a0 = -as;
v0 = vh;
s0 = X2(end,1);

for ti = 1:T/2/h
    x = fxj(jm,a0,v0,s0,ti*h);
    y = fyj(jm,a0,v0,s0,ti*h);
    X2(ti+L,3) = fzj2(jm,a0,v0,s0,ti*h);
    X3(ti+L,3) = fzj3(jm,a0,v0,s0,ti*h);
    X4(ti+L,3) = fzj4(jm,a0,v0,s0,ti*h);
    X5(ti+L,3) = fzj5(jm,a0,v0,s0,ti*h);


    X2(ti+L,1:2) = [x y];
    X3(ti+L,1:2) = [x y];
    X4(ti+L,1:2) = [x y];
    X5(ti+L,1:2) = [x y];

    v(ti+L,1) = vxj(jm,a0,v0,ti*h);
    v(ti+L,2) = vyj(jm,a0,v0,ti*h);
    v2(ti+L,3) = vzj2(jm,a0,v0,ti*h);
    v3(ti+L,3) = vzj3(jm,a0,v0,ti*h);
    v4(ti+L,3) = vzj4(jm,a0,v0,ti*h);
    v5(ti+L,3) = vzj5(jm,a0,v0,ti*h);

    a(ti+L,1) = axj(jm,a0,ti*h);
    a(ti+L,2) = ayj(jm,a0,ti*h);
    a2(ti+L,3) = azj2(jm,a0,ti*h);
    a3(ti+L,3) = azj3(jm,a0,ti*h);
    a4(ti+L,3) = azj4(jm,a0,ti*h);
    a5(ti+L,3) = azj5(jm,a0,ti*h);

    j(ti+L,1) = jx(jm);
    j(ti+L,2) = jy(jm);
    j2(ti+L,3) = jz2(jm);
    j3(ti+L,3) = jz3(jm);
    j4(ti+L,3) = jz4(jm);
    j5(ti+L,3) = jz5(jm);
end
L = length(X2);
L5 = length(X2);


time_t = t_lin + 2*T;
time = time + time_t;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% S V I S L Á   T R A J E K T O R I E  -  konec
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

vmaxs2 = vmax;
v0 = 0;
jmss = as^2/abs((vmaxs2-v0));

%% ZRYCHLOVÁNÍ (do záporu)
% ČÁST 1
jm = -jmss;
a0 = 0;
v0 = 0;
s0 = X2(end,3);

Ts2 = abs( 2*as/jm );

for ti = 1:Ts2/2/h % první a poslední část rozdělené na půl
    x = set(1);
    y = set(2);
    zs21 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs21];
end
L = length(X);
Ls21 = length(X);

% ZRYCHLOVÁNÍ - část 2
jm = jmss;
a0 = a(end,3);
v0 = v(end,3);
s0 = X(end,3);

for ti = 1:Ts2/2/h
    x = set(1);
    y = set(2);
    zs22 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs22];
end
L = length(X);
Ls22 = length(X);


    while abs( X(end,3) - X(L5+1,3) ) > 1/2*abs( set2(3) - set(3) )
        vmaxs2 = 0.9*vmaxs2;
        X(L5+1:end,:) = [];
        v(L5+1:end,:) = [];
        a(L5+1:end,:) = [];
        j(L5+1:end,:) = [];
        jmss = as^2/abs((vmaxs2-v0));

        % ČÁST 1
        jm = -jmss;
        a0 = 0;
        v0 = 0;
        s0 = set2(3);
        
        Ts2 = abs( 2*as/jm );

        for ti = 1:Ts2/2/h
            x = set(1);
            y = set(2);
            zs21 = fzj(jm,a0,v0,s0,ti*h);

            v(ti+L5,3) = vj(jm,a0,v0,ti*h);
            a(ti+L5,3) = aj(jm,a0,ti*h);
            j(ti+L5,3) = jm;
        
            X(ti+L5,:) = [x y zs21];
        end
        L = length(X);
        Ls21 = length(X);
        
        
        % ZRYCHLOVÁNÍ - část 2
        jm = jmss;
        a0 = a(end,3);
        v0 = v(end,3);
        s0 = X(end,3);
        
        for ti = 1:Ts2/2/h
            x = set(1);
            y = set(2);
            zs22 = fzj(jm,a0,v0,s0,ti*h);

            v(ti+L,3) = vj(jm,a0,v0,ti*h);
            a(ti+L,3) = aj(jm,a0,ti*h);
            j(ti+L,3) = jm;
        
            X(ti+L,:) = [x y zs22];
        end
        L = length(X);
        Ls22 = length(X);
        
    end


%% KONSTANTNÍ RYCHLOST
jm = 0;
a0 = 0;
v0 = v(end,3);
s0 = X(end,3);

% tirk = abs(abs(5*s52)-2*abs(zs22-set(3)))/(abs(vs22));
t_lins2 = abs(set2(3)-set(3)-2*abs( X(end,3)-set(3) ) ) / abs(v0);

for ti = 1:t_lins2/h % +1 aby to dojelo správně do koncového bodu
    x = set(1);
    y = set(2);
    zs23 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs23];
end
L = length(X);
Ls23 = length(X);

%% ZPOMALOVÁNÍ

% ČÁST 1
jm = jmss;
a0 = 0;
v0 = v(end,3);
s0 = X(end,3);

for ti = 1 : Ts2/2/h
    x = set(1);
    y = set(2);
    zs24 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs24];
end
L = length(X);
Ls24 = length(X);


% ČÁST 2
% Concave period
jm = -jmss;
a0 = a(end,3);
v0 = v(end,3);
s0 = X(end,3);

for ti = 1 : Ts2/2/h
    x = set(1);
    y = set(2);
    zs25 = fzj(jm,a0,v0,s0,ti*h);

    v(ti+L,3) = vj(jm,a0,v0,ti*h);
    a(ti+L,3) = aj(jm,a0,ti*h);
    j(ti+L,3) = jm;

    X(ti+L,:) = [x y zs25];
end
L = length(X);
Ls25 = length(X);

time = time + 2*Ts2 + t_lins2;



j2(L5+1:L,:) = j(L5+1:end,:); j3(L5+1:L,:) = j(L5+1:end,:); j4(L5+1:L,:) = j(L5+1:end,:); j5(L5+1:L,:) = j(L5+1:end,:);
a2(L5+1:L,:) = a(L5+1:end,:); a3(L5+1:L,:) = a(L5+1:end,:); a4(L5+1:L,:) = a(L5+1:end,:); a5(L5+1:L,:) = a(L5+1:end,:);
v2(L5+1:L,:) = v(L5+1:end,:); v3(L5+1:L,:) = v(L5+1:end,:); v4(L5+1:L,:) = v(L5+1:end,:); v5(L5+1:L,:) = v(L5+1:end,:);



%% ABSOLUTNÍ HODNOTY VELIČIN
% pol. st. 2
j2_abs = sqrt(j(:,1).^2 + j(:,2).^2 + j2(:,3).^2);
a2_abs = sqrt(a(:,1).^2 + a(:,2).^2 + a2(:,3).^2);
v2_abs = sqrt(v(:,1).^2 + v(:,2).^2 + v2(:,3).^2);

j2_a = max(j2_abs);
a2_a = max(a2_abs);
v2_a = max(v2_abs);

% pol. st. 3
j3_abs = sqrt(j(:,1).^2 + j(:,2).^2 + j3(:,3).^2);
a3_abs = sqrt(a(:,1).^2 + a(:,2).^2 + a3(:,3).^2);
v3_abs = sqrt(v(:,1).^2 + v(:,2).^2 + v3(:,3).^2);

j3_a = max(j3_abs);
a3_a = max(a3_abs);
v3_a = max(v3_abs);

% pol. st. 2
j4_abs = sqrt(j(:,1).^2 + j(:,2).^2 + j4(:,3).^2);
a4_abs = sqrt(a(:,1).^2 + a(:,2).^2 + a4(:,3).^2);
v4_abs = sqrt(v(:,1).^2 + v(:,2).^2 + v4(:,3).^2);

j4_a = max(j4_abs);
a4_a = max(a4_abs);
v4_a = max(v4_abs);

% pol. st. 2
j5_abs = sqrt(j(:,1).^2 + j(:,2).^2 + j5(:,3).^2);
a5_abs = sqrt(a(:,1).^2 + a(:,2).^2 + a5(:,3).^2);
v5_abs = sqrt(v(:,1).^2 + v(:,2).^2 + v5(:,3).^2);

j5_a = max(j5_abs);
a5_a = max(a5_abs);
v5_a = max(v5_abs);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% N E J L E P Š Í   D R Á H A %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

X2 = [X2; X(L5+1:end,:)];
X3 = [X3; X(L5+1:end,:)];
X4 = [X4; X(L5+1:end,:)];
X5 = [X5; X(L5+1:end,:)];


x2er = 0;
x3er = 0;
x4er = 0;
x5er = 0;
for st = 1 : stoh.pocet
    for ax = 1 : numel(X5(Ls15+1:L5,1))
        if abs( X2(Ls15+ax,2) - stoh.poloha(st,2) ) < stoh.sirka*1.05
            if abs( X2(Ls15+ax,1) - stoh.poloha(st,1) ) < stoh.delka*1.05
                if X2(Ls15+ax,3) < stoh.vyska(st)+0.1
                    x2er = 1;
                end
            end
        end

        if abs( X3(Ls15+ax,2) - stoh.poloha(st,2) ) < stoh.sirka*1.05
            if abs( X3(Ls15+ax,1) - stoh.poloha(st,1) ) < stoh.delka*1.05
                if X3(Ls15+ax,3) < stoh.vyska(st)+0.1
                    x3er = 1;
                end
            end
        end

        if abs( X4(Ls15+ax,2) - stoh.poloha(st,2) ) < stoh.sirka*1.05
            if abs( X4(Ls15+ax,1) - stoh.poloha(st,1) ) < stoh.delka*1.05
                if X4(Ls15+ax,3) < stoh.vyska(st)+0.1
                    x4er = 1;
                end
            end
        end

        if abs( X5(Ls15+ax,2) - stoh.poloha(st,2) ) < stoh.sirka*1.05
            if abs( X5(Ls15+ax,1) - stoh.poloha(st,1) ) < stoh.delka*1.05
                if X5(Ls15+ax,3) < stoh.vyska(st)+0.1
                    x5er = 1;
                end
            end
        end 
    end
end

for ax = 1 : numel(X5(:,1))
    if X2(ax,3) > maxvyska
        x2er = 1;
    end
    
    if X3(ax,3) > maxvyska
        x3er = 1;
    end

    if X4(ax,3) > maxvyska
        x4er = 1;
    end

    if X5(ax,3) > maxvyska
        x5er = 1;
    end
end



if x2er == 1
    if x3er == 1
        if x4er == 1
            if x5er == 1
                disp('E R R O R  -  D R Á H A   N E V Y K R E S L E N A')
            elseif x5er == 0
                Xtraj = X5;
                v(Ls15+1 : L5,3) = v5(Ls15+1 : L5,3);
                a(Ls15+1 : L5,3) = a5(Ls15+1 : L5,3);
                j(Ls15+1 : L5,3) = j5(Ls15+1 : L5,3);
            end
        elseif x4er == 0
            Xtraj = X4;
            v(Ls15+1 : L5,3) = v4(Ls15+1 : L5,3);
            a(Ls15+1 : L5,3) = a4(Ls15+1 : L5,3);
            j(Ls15+1 : L5,3) = j4(Ls15+1 : L5,3);
        end
    elseif x3er == 0
        Xtraj = X3;
        v(Ls15+1 : L5,3) = v3(Ls15+1 : L5,3);
        a(Ls15+1 : L5,3) = a3(Ls15+1 : L5,3);
        j(Ls15+1 : L5,3) = j3(Ls15+1 : L5,3);
    end
elseif x2er == 0
    Xtraj = X2;
    v(Ls15+1 : L5,3) = v2(Ls15+1 : L5,3);
    a(Ls15+1 : L5,3) = a2(Ls15+1 : L5,3);
    j(Ls15+1 : L5,3) = j2(Ls15+1 : L5,3);
end

j_abs = sqrt(j(:,1).^2 + j(:,2).^2 + j(:,3).^2);
a_abs = sqrt(a(:,1).^2 + a(:,2).^2 + a(:,3).^2);
v_abs = sqrt(v(:,1).^2 + v(:,2).^2 + v(:,3).^2);

j_a = max(j_abs);
a_a = max(a_abs);
v_a = max(v_abs);




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% D A T A   P R O   S I M S C A P E %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

timepoints = (linspace(0,time,numel(Xtraj(:,1))))';

xtraj = [timepoints, Xtraj(:,1)];
ytraj = [timepoints, Xtraj(:,2)];
ztraj = [timepoints, Xtraj(:,3)-robot.vyska_z];





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   F   U   N   K   C   E   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mezi] = vymazanimezi(mezi)
mezif = mezi;
mn = 1;
for m = 1 : numel(mezif(:,1))
    if mezif(m,:) == [0 0 0]
        mezi(mn,:) = [];
        mn = mn-1;
    end
    mn = mn + 1;
end
end

