clear all, clc

a_a = 20;
a_d = 20;
v_p = 20;
v0 = 0;
v_konc = 0;
x = 50;
x0 = 0;
dt = 0.001;
l = 10000;
%------------------------------------------------
x_p = x;
%v_konc_p = -v0 + v_konc; % ++
%v = v_p-v0;
%vk = v-v_konc_p;  % ++

t_a1 = abs((v_p-v0)/a_a); %+++++++++++++++++++++++
t_d11 = abs((v_p-v0)/a_d); %+++++++++++++++++++++++
t_d1 = (v_p-v_konc)/a_d;
t_ad = t_a1 + t_d1;

v_traj = sqrt((x-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))));

%t1 = (a_d/a_a)*sqrt((2*x)/((power(a_d,2)/a_a)+a_d));
%t2 = sqrt((2*x)/((power(a_d,2)/a_a)+a_d));
% p = (power(a_d,2)/(2*a_a))+(a_d/2);     %koeficient "a" u kvadr. rovnice
% q = (a_d/a_a)*(v_konc_p);                   %koeficient "b" u kvadr. rovnice
% r = (power(v_konc_p,2)/(2*a_a))-x;        %koeficient "c" u kvadr. rovnice
% D = power(q,2) - (4*p*r);               %diskriminant  

% t2 = (-q + sqrt(D))/(2*p);             % cas zpomaleni
% t2_minus = (-q - sqrt(D))/(2*p);
% t1 = (1/a_a)*((a_d*t2)+v_konc_p);         %cas zrychleni

t1 = abs((v_traj-v0)/a_a);
t11 = abs((v_traj-v0)/a_d);
t2 = (v_traj-v_konc)/a_d;

t12 = t1 + t2;

if v0 > v_p
t_a1 = t_d11;      % ++++
t1 = t11;    
end

if t12 < t_ad
    t_a = t1;
    %t_ad = t11;
    t_d = t2;
    v_prom = v_traj;
else
    t_a = t_a1;
   % t_ad = t_d11;
    t_d = t_d1;
    v_prom = v_p;
end

%v_dos = v0 + a_a*t_a;

if v0 < v_p
s_a = 0.5*a_a*power(t_a,2)+v0*t_a;      % ++++
else
s_a = -0.5*a_d*power(t_a,2)+v0*t_a;      % ++++    
end
s_d = v_prom*t_d - 0.5*a_d*power(t_d,2);   % ++++
s_v = x - s_a - s_d;

t_sa = (-v0 + sqrt(power(v0,2)+(2*a_a*x_p)))/(a_a);
t_sd = (v0 - sqrt(power(v0,2)-(2*a_d*x_p)))/(a_d);

if x_p <= s_a && v0 < v_konc
    %a_kor = (2/power(t_a,2))*(x_p - v0*t_a);
    disp('Pozadovane rychlosti nelze na dane draze dosahnout');
%     disp(['Bylo upraveno zrychleni a_a na hodnotu ',num2str(a_kor),' mm/s2'])
%     a_a = a_kor;
    
    disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti');
    disp(' ');
    %error('Pozadovane rychlosti nelze na dane draze dosahnout')
elseif x_p <= s_d && v0 > v_konc && x_p ~= 0
    %a_kor = (2/power(t_d,2))*(v0*t_d - x_p);
    %a_kor = (v0 - v_konc)/t_d%(2*(0-x_p))
    %disp('Pozadovane rychlosti nelze na dane draze dosahnout');
    %disp(['Bylo upraveno zrychleni a_a na hodnotu ',num2str(a_kor),' mm/s2'])
    %a_d = a_kor;
    disp('Pozadovane rychlosti nelze na dane draze dosahnout');
    disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti');
    disp(' ');
    %error('Pozadovane rychlosti nelze na dane draze dosahnout')
end


t_v = s_v / v_p;

if t_d < 0
    t_d = 0;
elseif t_a < 0
    t_a = 0;
end
t_celk = t_a + t_v + t_d;




% a1 = ones(1,n);
% a2 = zeros(1,(2*n)-1);
% a3 = (-1)* ones(1,n);
% a4 = zeros(1,round(n/8));
% 
% a = a0*[0 a1 a2 a3 a4];

%t_dop = [ ];
v1(1) = v0;
x1(1) = x0;
t_dop(1)=0;
i = 2;
t_dec(1) = 0;
%zrychlení
%for i = 2:l
while t_dop < t_celk
    t_dec(i) = -dt;
    t_dec1(i) = 0;
    t_dop(i) = t_dop(i-1)+dt;
    t_obr(i) = t_celk - t_dop(i);
    
    
    if t_dop(i) >= t_a
        if t_obr(i) < 0
            a(i) = 0;
            s5(i) = x;
            %s5(i) = v_prom*t_dop(i)-s_a;
        else
            if t_obr(i) <= t_d
                t_dec(i) = t_dec(i-1)+dt;
                a(i) = -a_d;
                s5(i) = (x-s_d) + v_prom*t_dec(i) - 0.5*a_d*power(t_dec(i),2);
            else
                a(i) = 0;
                t_dec1(i) = t_dec1(i-1)+dt;
                s5(i) = v_prom*t_dec1(i)+s_a;
            end
        end
    else
        if v_p < v0
            a(i) = -a_d;
            s5(i) = v0*t_dop(i) - 0.5*a_d*power(t_dop(i),2);
        else
            a(i) = a_a;
            s5(i) = v0*t_dop(i) + 0.5*a_a*power(t_dop(i),2);
        end
    end
    

    
    v1(i) = v1(i-1)+a(i)*dt;
    x1(i) = x1(i-1) + 0.5*(v1(i)+v1(i-1))*dt;
    
    v5(i) = (s5(i)-s5(i-1))/dt;
    a5(i) = (v5(i)-v5(i-1))/dt;
    
    i = i+1;
end

v = v1;
x = x1;

t_c = length(x1);
t = dt*[0 1 : t_c-1];

%chyba
E1 = power(dt,3)*(t_a/dt)*max(a)/12;
E3 = power(dt,3)*(t_d/dt)*max(a)/12;
E = E1 + E3;

Ep = abs(x1-s5);
%--------------------------

disp(['Poloha v èase t, s = ',num2str(x(t_c)),' mm'])
disp(['Rychlost v èase t, v = ',num2str(v(t_c)),' mm/s'])
disp(['Zrychlení v èase t, a = ',num2str(a(t_c)),' mm/s2'])

disp(['Èas t = ',num2str(t(t_c)),' s'])
disp(' ')
disp(['Chyba numerické integrace dráhy, E = ',num2str(E),' mm'])

%disp(['Poloha v èase t_celk, s = ',num2str(x(t_celk)),' mm'])

w = 5;

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

%figure(2)
subplot(3,1,2);plot(t,v,'k','linewidth',w)
hold on
plot([t_celk t_celk 0],[0 v_konc v_konc],'r --')
plot(t,v5,'b')
grid on
title('Prùbìh rychlosti')
xlabel('t [s]')
ylabel('v [mm/s]')
xlim([0  t(length(t))]);
ylim([-1.5*v0  1.5*max(v)]);
hold off

%figure(3)
subplot(3,1,3);plot(t,x,'k','linewidth',w)
hold on
plot([t_celk t_celk 0],[0 x_p x_p],'r --')
plot(t,s5,'b')
grid on
title('Prùbìh souøadnice polohy')
xlabel('t [s]')
ylabel('s [mm]')
%xlim([0  t(length(t))]);
%ylim([-1.5*x0  1.5*max(x)]);

figure(2)
plot(t,Ep)

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

subplot(2,1,2);plot(t,x,'k','linewidth',w)
hold on
plot([t_celk t_celk 0],[0 x_p x_p],'r --')
grid on
title('Prùbìh souøadnice polohy')
xlabel('t [s]')
ylabel('s [mm]')