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

M_pr_ef=149.5
M_pr_ef_grad=479.9

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

% Zrychleni
a=[];
a_brz=[];
for i=1:n
    % Gradient = 0
    if alfa(i)==0
        ai=      (2/r_kol*(M_pr_ef*i_r*eta_r-0.5*ksi_val*m*g*cos(alfa(i)*pi/180))-m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
        ai_brz=  (2/r_kol*(M_pr_ef*i_r*eta_r+0.5*ksi_val*m*g*cos(alfa(i)*pi/180))+m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
    elseif alfa(i)>0
        ai=      (2/r_kol*(M_pr_ef_grad*i_r*eta_r-0.5*ksi_val*m*g*cos(alfa(i)*pi/180))-m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
        ai_brz=  (2/r_kol*(M_pr_ef*i_r*eta_r+0.5*ksi_val*m*g*cos(alfa(i)*pi/180))+m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
    else
        ai=      (2/r_kol*(M_pr_ef*i_r*eta_r-0.5*ksi_val*m*g*cos(alfa(i)*pi/180))-m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
        ai_brz=  (2/r_kol*(M_pr_ef_grad*i_r*eta_r+0.5*ksi_val*m*g*cos(alfa(i)*pi/180))+m*g*sin(alfa(i)*pi/180))/(2*((i_pr^2*r2^2)/(r1^2*r_kol^2)*I0)+2*(r2^2/(r1^2*r_kol^2)*I1)+4*(r2^2/r_kol^2*mr)+4/r_kol^2*(I2+IH+IK)+m);
    end
    a=[a;ai];
    a_brz=[a_brz;ai_brz];
end

% Rychlost startovni
v_s=[ v_s1; v_f ];

% Casy
Ts=[ T_s1 ];
T1=[ ];
T2=[ ];
T3=[ ];
Tf=[ ];
a_skut=[]; 
for i=1:n
    disp(i)
    if a(i)<0
        disp(i)
        disp('Nedostatecny krouici moment na kole. Stroj nevyjede kopec.')
        break
    elseif a_brz(i)<0
        disp(i)
        disp('Nedostatecny brzdny moment. Stroj neubrzdi kopec.')
        break
    end
    
    %Charakteristika trajektorie v intervalu 1
    if v_s(i)<v_max(i)
        a1=a(i);
    elseif v_s(i)>v_max(i)
        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);
    elseif v_max(i)>v_f(i)
        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);
    if T2i<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
    Tfi=T1i+T2i+T3i;
    
    if a1<0
        T3ii=(v_max(i)-v_s(i))/a1/3.6;
    else
        T3ii=0;
    end
    
    if a3<0
        T1ii=(v_f(i)-v_max(i))/a3/3.6;
    else
        T1ii=0;
    end
     
    a_skut_i=[a1 a2 a3];
    a_skut=[a_skut; a_skut_i];
    
    T1=[ T1; T1i+T1ii ];
    T2=[ T2; T2i ];
    T3=[ T3; T3i+T3ii ];
    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))

% Rychlost
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
            if alfa(i)==0
                M_ij=-M_pr_ef;
            else
                M_ij=-M_pr_ef_grad;
            end
        else
            if alfa(i)==0
                M_ij=M_pr_ef;
            else
                M_ij=M_pr_ef_grad;
            end
        end
        M_i=[M_i;M_ij];
        k=k+1;
    end
    M_pr=[M_pr;M_i];
end
M_m=M_pr/i_pr/eta_pr;

% Vykon
P_motor=0.001*M_m.*v/3.6*r2/r1/r_kol*i_pr;

P_m=[];
% Vykon na vystupu motoru
k=1;
for i=1:n
    t_i=[0:t_inc:floor(Tf(i))]';
    for j=1:length(t_i)
        if P_motor(k)>Pm_max(i)
            Pm=Pm_max(i);
        elseif P_motor(k)<-Pm_brmax(i)
            Pm=-Pm_brmax(i);
        else
            Pm=2*P_motor(k);
        end
        k=k+1;
        P_m=[P_m;Pm];
    end
end

% Rychlost
v=[];
for i=1:length(P_m)
    vi=1000*0.5*P_m(i)./(i_pr*r2/r1/r_kol*M_m(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];