global alfa L v_f v_max n m r_kol mu_a g yT xTA xTB eta_val M_prmax T_s1 T_sum

M_pr_provozni=M_prmax_celk

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vektorove parametry
a=      (2*(M_pr_provozni.*i_r*eta_r-eta_val*(0.5*m*g.*cos(alfa*pi/180)))/r_kol-m*g.*sin(alfa*pi/180))/(m+2*I1*r2^2/r1^2/r_kol^2+4*(mr*r2^2+I2+IH+IK)/r_kol^2);
a_brz=  (2*(M_pr_provozni.*i_r*eta_r+eta_val*(0.5*m*g.*cos(alfa*pi/180)))/r_kol+m*g.*sin(alfa*pi/180))/(m+2*I1*r2^2/r1^2/r_kol^2+4*(mr*r2^2+I2+IH+IK)/r_kol^2);
v_s=[ v_s1; v_f ];

%Casy
Ts=[ T_s1 ];
T1=[ ];
T2=[ ];
T3=[ ];
Tf=[ ];

% skutecne zrychleni
a_skut=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
for i=1:n
    if a(i)<0
        disp('Nedostatecny krouici moment na kole. Stroj nevyjede kopec.')
        break
    elseif a_brz(i)<0
        disp('Nedostatecny brzdny moment. Stroj neubrzdi kopec.')
        break
    end
    
    %Charakteristika trajektorie v intervalu 1
    if v_max(i)>v_s(i)
        a1=a(i);
    else
        a1=-a_brz(i);
    end      
    
    % Charakteristika trajektorie v intervalu 2
    a2=0;
    
    %Charakteristika trajektorie v intervalu 3
    if v_max(i)<v_f(i)
        a3=a(i);
    else
        a3=-a_brz(i);
    end
      
    T1i=(v_max(i)-v_s(i))/a1/3.6;
    T3i=(v_f(i)-v_max(i))/a3/3.6;
    T2i=(L(i)-(v_s(i)/3.6*T1i)-(v_max(i)/3.6*T3i)-0.5*a1*T1i^2-0.5*a3*T3i^2)/(v_max(i)/3.6);
    Tfi=T1i+T2i+T3i;
    
    if a1<0
        T3i=T1i;
    elseif a3<0
        T1i=T3i;
    end
    
    if T2i<0
        disp('T2<0')
        v12i=3.6*(Tfi*a1*a3+(v_s(i)*a3-v_f(i)*a1)/3.6)/(a3-a1);
        T12i=(v12i-v_s(i))/a1/3.6;
        T1i=T12i;
        T2i=0;
        T3i=Tfi-T1i;
    end
    
    a_skut_i=[a1 a2 a3];
    a_skut=[a_skut; a_skut_i];
    
    
    
    T1=[ T1; T1i ];
    T2=[ T2; T2i ];
    T3=[ T3; T3i ];
    
    Tf=[ Tf; Tfi ];
end

Ts=[ Ts; Tf ];
T=[T1 T2 T3];
T0=[[0 0 0]; T];
T1_sum=sum(T1)
T2_sum=sum(T2)
T3_sum=sum(T3)
T_sum=sum(sum(T))

t_v=[];
v=[];
for i=1:n %Lokalni souradnicovy system t-v, odkud merime cas v i-tem useku
    t_i=[0:t_inc:floor(Tf(i))]';
    v_i=[];
    a_ti=[];
    for j=1:length(t_i)
        % Rychlost
        if t_i(j)<T1(i)
            v_ij=v_s(i)+a_skut(i,1)*t_i(j);
        elseif T1(i)+T2(i)<t_i(j)
            v_ij=v_max(i)+a_skut(i,3)*(t_i(j)-T1(i)-T2(i));
        else
            v_ij=v_max(i);
        end      
        v_i=[v_i; v_ij];
    end
    t_v=[t_v; t_i+sum(Ts(1:i))];
    v=[v; v_i];
end

% Zrychleni
a_t=[];
k=1;
for i=1:n 
    a_ti=[];
    t_i=[0:t_inc:floor(Tf(i))]';
    for j=1:length(t_i)
        a_tij=(v(k+1)-v(k))/t_inc;
        if a_tij>a(i)
            a_tij=a(i);
        elseif a_tij<-a_brz(i)
            a_tij=-a_brz(i);
        end
        a_ti=[a_ti;a_tij];
        k=k+1;
        if k==length(v)
            break
        end
    end
    a_t=[a_t;a_ti];
end
a_t=[a_t;nan];

% Moment
M_pr=[];
k=1;
for i=1:n
    M_i=[];
    t_i=[0:t_inc:floor(Tf(i))]';
    for j=1:length(t_i)
        if a_t(k)==0
            M_ij=M_pr_u(i);
        elseif a_t(k)<0
            M_ij=-M_pr_provozni;
        else
            M_ij=M_pr_provozni;
        end
        M_i=[M_i;M_ij];
        k=k+1;
    end
    M_pr=[M_pr;M_i];
end

% Vykon
P_prevodovka=0.001*2*M_pr.*v/3.6*r2/r1/r_kol;

P_pr=[];
P_beh=[];

% Vykon na vystupu prevodovky
k=1;
for i=1:n
    t_i=[0:t_inc:floor(Tf(i))]';
    for j=1:length(t_i)
        if P_prevodovka(k)>P_max_pr(i)
            Ppr=P_max_pr(i);
        elseif P_prevodovka(k)<-P_brmax_pr(i)
            Ppr=-P_brmax_pr(i);
        else
            Ppr=P_prevodovka(k);
        end
        k=k+1;
        P_pr=[P_pr;Ppr];
    end
end

% Rychlost
v=[];
for i=1:length(P_pr)
    vi=1000*0.5*P_pr(i)./(r2/r1/r_kol*M_pr(i))*3.6;
    if vi>max(v_max)
        vi=max(v_max);
    end
    v=[v;vi];
end

% Zrychleni 2
a_t=[];
k=1;
for i=1:n
    a_ti=[];
    t_i=[0:t_inc:floor(Tf(i))]';
    for j=1:length(t_i)
        a_tij=(v(k+1)-v(k))/t_inc;
        if a_tij>a(i)
            a_tij=a(i);
            v(k+1)=v(k)+a_tij*t_inc;
        elseif a_tij<-a_brz(i)
            a_tij=-a_brz(i);
            v(k+1)=v(k)+a_tij*t_inc;
        end
        a_ti=[a_ti;a_tij];
        k=k+1;
        if k==length(v)
            break
        end
    end
    a_t=[a_t;a_ti];
end
a_t=[a_t;nan];