close all; clear all; clc;

%Zadání parametrù výpoètu:
krok=0.1;                       %Délka kroku výpoètu
roz=1.435/3.5;                  %Rozchod v [m]
dp=2.5/3.5;                     %Rozvor podvozku v [m]
ss=1500/3.5;                    %Vzdálenost míst dotyku kola a kladky [m]

%Zadání parametrù kladky
dk=0.5;                         %Prùmìr kladek [m]


%*************************************
%Naètení trati ze souboru

A=importdata(uigetfile('*.*'),'');
j=size(A,1);
Horizontal=zeros(1,4);
Vertical=Horizontal;
Superel=Horizontal;
ZmenaRozchodu=Horizontal;
k=1;
l=1;
kk=1;
ll=1;

for i=1:j
    h=char(A(i));
    if(size(h,2)>4)
        if(strcmp(h(4),'H')==1)
            g=strsplit(h);
            if(strcmp(h(9:11),'STR')==1)
                Horizontal(k,1)=1;
                Horizontal(k,2)=str2double(char(g(4)));
            end
            if(strcmp(h(9:11),'CIR')==1)
                Horizontal(k,1)=2;
                Horizontal(k,2)=str2double(char(g(4)));
                Horizontal(k,3)=str2double(char(g(5)));
            end
            if(strcmp(h(9:11),'CLO')==1)
                Horizontal(k,1)=3;
                Horizontal(k,2)=str2double(char(g(4)));
                Horizontal(k,3)=str2double(char(g(5)));
                Horizontal(k,4)=str2double(char(g(6)));
            end
            k=k+1;
        end
        
        if(strcmp(h(4),'V')==1)
            g=strsplit(h);
            if(strcmp(h(9:11),'CSL')==1)
                Vertical(kk,1)=1;
                Vertical(kk,2)=str2double(char(g(4)));
                Vertical(kk,3)=str2double(char(g(5)));
            end
            if(strcmp(h(9:11),'PL2')==1)
                Vertical(kk,1)=2;
                Vertical(kk,2)=str2double(char(g(4)));
                Vertical(kk,3)=str2double(char(g(5)));
                Vertical(kk,4)=str2double(char(g(6)));
            end
            kk=kk+1;
        end
        
        if(strcmp(h(4),'S')==1)
            g=strsplit(h);
            if(strcmp(h(9:11),'CST')==1)
                Superel(l,1)=1;
                Superel(l,2)=str2double(char(g(4)));
                Superel(l,3)=str2double(char(g(5)));
            end
            if(strcmp(h(9:11),'LIR')==1)
                Superel(l,1)=2;
                Superel(l,2)=str2double(char(g(4)));
                Superel(l,3)=str2double(char(g(5)));
                Superel(l,4)=str2double(char(g(6)));
            end
            l=l+1;
        end
        if(strcmp(h(1),'S')==1)
            g=strsplit(h);
            if(strcmp(h(6:8),'CST')==1)
                Superel(l,1)=1;
                Superel(l,2)=str2double(char(g(3)));
                Superel(l,3)=str2double(char(g(4)));
            end
            if(strcmp(h(6:8),'LIR')==1)
                Superel(l,1)=2;
                Superel(l,2)=str2double(char(g(3)));
                Superel(l,3)=str2double(char(g(4)));
                Superel(l,4)=str2double(char(g(5)));
            end
            l=l+1;
        end
        
        if(strcmp(h(4),'U')==1)
            g=strsplit(h);
            if(strcmp(h(9:11),'CON')==1)
                ZmenaRozchodu(ll,1)=1;
                ZmenaRozchodu(ll,2)=str2double(char(g(4)));
                ZmenaRozchodu(ll,3)=str2double(char(g(5)));
            end
            if(strcmp(h(9:11),'LIN')==1)
                ZmenaRozchodu(ll,1)=2;
                ZmenaRozchodu(ll,2)=str2double(char(g(4)));
                ZmenaRozchodu(ll,3)=str2double(char(g(5)));
                ZmenaRozchodu(ll,4)=str2double(char(g(6)));
            end
            ll=ll+1;
        end
        if(strcmp(h(1),'U')==1)
            g=strsplit(h);
            if(strcmp(h(6:8),'CON')==1)
                ZmenaRozchodu(ll,1)=1;
                ZmenaRozchodu(ll,2)=str2double(char(g(3)));
                ZmenaRozchodu(ll,3)=str2double(char(g(4)));
            end
            if(strcmp(h(6:8),'LIN')==1)
                ZmenaRozchodu(ll,1)=2;
                ZmenaRozchodu(ll,2)=str2double(char(g(3)));
                ZmenaRozchodu(ll,3)=str2double(char(g(4)));
                ZmenaRozchodu(ll,4)=str2double(char(g(5)));
            end
            ll=ll+1;
        end
    end
end

clear A

%*************************************
%Výpoèet hodnot pro vykreslení trati do grafu XY

XYZ=[0;0];
j=size(Horizontal,1);
fi=zeros(j,1);

for  i=1:j
    
    %Rovný úsek
    if(Horizontal(i,1)==1)
        fi(i)=0;
        X=krok:krok:Horizontal(i,2);
        Y=zeros(size(X));
    end
    
    %Oblouk
    if(Horizontal(i,1)==2)
        fi(i)=Horizontal(i,2)/Horizontal(i,3);
        
        %Oblouk doleva
        %Mezi 180° a 270°
        if(fi(i)>pi)
            d=krok:krok:Horizontal(i,3)*pi;
            d1=size(d,2);
            d=krok:krok:Horizontal(i,3)*(fi(i)-pi);
            d2=size(d,2);
            Y1=2*Horizontal(i,3)/d1:2*Horizontal(i,3)/d1:2*Horizontal(i,3);
            Y2=2*Horizontal(i,3):-(2*Horizontal(i,3)-Horizontal(i,3)*(1-cos(fi(i))))/d2:Horizontal(i,3)*(1-cos(fi(i)));
            X1=sqrt(2*Horizontal(i,3).*Y1-(Y1).^2);
            X2=-sqrt(2*Horizontal(i,3).*Y2-(Y2).^2);
            X=[X1,X2];
            Y=[Y1,Y2];
        %Mezi 90° a 180°
        elseif(pi>=fi(i) && fi(i)>pi/2)
            d=krok:krok:Horizontal(i,2);
            d1=size(d,2);
            Y=Horizontal(i,3)*(1-cos(fi(i)))/d1:Horizontal(i,3)*(1-cos(fi(i)))/d1:Horizontal(i,3)*(1-cos(fi(i)));
            X=sqrt(2*Horizontal(i,3).*Y-(Y).^2);
        %Mezi 0° a 90°
        elseif(fi(i)<=pi/2 && fi(i)>=0)
            d=krok:krok:Horizontal(i,2);
            d1=size(d,2);
            Y=Horizontal(i,3)*(1-cos(fi(i)))/d1:Horizontal(i,3)*(1-cos(fi(i)))/d1:Horizontal(i,3)*(1-cos(fi(i)));
            X=sqrt(2*Horizontal(i,3).*Y-(Y).^2);
        end
        
        %Oblouk doprava
        %Mezi 180° a 270°
        if(fi(i)<-pi)
            d=krok:krok:-Horizontal(i,3)*pi;
            d1=size(d,2);
            d=krok:krok:-Horizontal(i,3)*(-fi(i)-pi);
            d2=size(d,2);
            Y1=2*-Horizontal(i,3)/d1:2*-Horizontal(i,3)/d1:2*-Horizontal(i,3);
            Y2=-2*Horizontal(i,3):-(2*-Horizontal(i,3)+Horizontal(i,3)*(1-cos(fi(i))))/d2:-Horizontal(i,3)*(1-cos(fi(i)));
            X1=sqrt(2*-Horizontal(i,3).*Y1-(Y1).^2);
            X2=-sqrt(2*-Horizontal(i,3).*Y2-(Y2).^2);
            X=[X1,X2];
            Y=[-Y1,-Y2];
        %Mezi 90° a 180°
        elseif(-pi<=fi(i) && fi(i)<-pi/2)
            d=krok:krok:Horizontal(i,2);
            d1=size(d,2);
            Y=-Horizontal(i,3)*(1-cos(fi(i)))/d1:-Horizontal(i,3)*(1-cos(fi(i)))/d1:-Horizontal(i,3)*(1-cos(fi(i)));
            X=sqrt(2*-Horizontal(i,3).*Y-(Y).^2);
            Y=-Y;
        %Mezi 0° a 90°
        elseif(fi(i)>=-pi/2 && fi(i)<0)
            d=krok:krok:Horizontal(i,2);
            d1=size(d,2);
            Y=-Horizontal(i,3)*(1-cos(fi(i)))/d1:-Horizontal(i,3)*(1-cos(fi(i)))/d1:-Horizontal(i,3)*(1-cos(fi(i)));
            X=sqrt(2*-Horizontal(i,3).*Y-(Y).^2);
            Y=-Y;
        end
        
    end
    
    %Klotoida
    if(Horizontal(i,1)==3)
        %Vstupní
        if(abs(Horizontal(i,4))>=abs(Horizontal(i,3)))
            fi(i)=Horizontal(i,2)/Horizontal(i,4)/2;
            if(fi(i)>=0)
                d=(krok:krok:Horizontal(i,2));
                X=d-d.^5/(40*Horizontal(i,2)^2*Horizontal(i,4)^2)+d.^9/(3456*Horizontal(i,2)^4*Horizontal(i,4)^4);
                Y=d.^3/(6*Horizontal(i,2)*Horizontal(i,4))-d.^7/(336*Horizontal(i,2)^3*Horizontal(i,4)^3)+d.^11/(3456*Horizontal(i,2)^5*Horizontal(i,4)^5);
            else
                d=(krok:krok:Horizontal(i,2));
                X=d-d.^5/(40*Horizontal(i,2)^2*Horizontal(i,4)^2)+d.^9/(3456*Horizontal(i,2)^4*Horizontal(i,4)^4);
                Y=d.^3/(6*Horizontal(i,2)*-Horizontal(i,4))-d.^7/(336*Horizontal(i,2)^3*-Horizontal(i,4)^3)+d.^11/(3456*Horizontal(i,2)^5*-Horizontal(i,4)^5);
                Y=-Y;
            end
        %Výstupní
        else
            fi(i)=Horizontal(i,2)/Horizontal(i,3)/2;
            if(fi(i)>0)
                d=(-krok:-krok:-Horizontal(i,2));
                X=d-d.^5/(40*Horizontal(i,2)^2*Horizontal(i,3)^2)+d.^9/(3456*Horizontal(i,2)^4*Horizontal(i,3)^4);
                Y=d.^3/(6*Horizontal(i,2)*Horizontal(i,3))-d.^7/(336*Horizontal(i,2)^3*Horizontal(i,3)^3)+d.^11/(42240*Horizontal(i,2)^5*Horizontal(i,3)^5);
                XK=Horizontal(i,2)-Horizontal(i,2)^3/(40*Horizontal(i,3)^2)+Horizontal(i,2)^5/(3456*Horizontal(i,3)^4);
                YK=Horizontal(i,2)^2/(6*Horizontal(i,3))-Horizontal(i,2)^4/(336*Horizontal(i,3)^3)+Horizontal(i,2)^6/(42240*Horizontal(i,3)^5);
            else
                d=(-krok:-krok:-Horizontal(i,2));
                X=d-d.^5/(40*Horizontal(i,2)^2*Horizontal(i,3)^2)+d.^9/(3456*Horizontal(i,2)^4*Horizontal(i,3)^4);
                Y=d.^3/(6*Horizontal(i,2)*-Horizontal(i,3))-d.^7/(336*Horizontal(i,2)^3*-Horizontal(i,3)^3)+d.^11/(42240*Horizontal(i,2)^5*-Horizontal(i,3)^5);
                XK=Horizontal(i,2)-Horizontal(i,2)^3/(40*Horizontal(i,3)^2)+Horizontal(i,2)^5/(3456*Horizontal(i,3)^4);
                YK=Horizontal(i,2)^2/(6*-Horizontal(i,3))-Horizontal(i,2)^4/(336*-Horizontal(i,3)^3)+Horizontal(i,2)^6/(42240*-Horizontal(i,3)^5);
                Y=-Y;
                YK=-YK;
            end
            
            s=size(Y,2);
                for l=1:s
                    Tp=[X(l)+XK;Y(l)+YK];
                    T=[ cos(-fi(i))   -sin(-fi(i))  ;
                        sin(-fi(i))    cos(-fi(i))  ];
                    Tp=T*Tp;
                    X(l)=Tp(1);
                    Y(l)=-Tp(2);
                end
            X=fliplr(X);
            Y=fliplr(Y);
        end
    end   
        
        
    %Otoèení úsekù
    u=size(XYZ,2);   
    s=size(X,2);
        for l=1:s
            Tp=[X(l);Y(l)];
            for k=1:i-1
                T=[ cos(fi(k))   -sin(fi(k))  ;
                    sin(fi(k))    cos(fi(k))  ];
                Tp=T*Tp;
            end
            X(l)=Tp(1)+XYZ(1,u);
            Y(l)=Tp(2)+XYZ(2,u);
        end


    
    XYZ=[XYZ(1,:)    X    ;
         XYZ(2,:)    Y     ];
    
end  
    
%************************************
%Výškové posunutí ve smìru ZV

k=size(Vertical,1);

for i=1:k
    %Konstantní sklon
    if(Vertical(i,1)==1)
        d=krok:krok:Vertical(i,2);
        zv=Vertical(i,3)*d/1000;
    end
    %Zvìtšování sklonu
    if(Vertical(i,1)==2 && abs(Vertical(i,3))<abs(Vertical(i,4)))
        d=krok:krok:Vertical(i,2);
        sk=Vertical(i,3)+(Vertical(i,4)-Vertical(i,3))/size(d,2):(Vertical(i,4)-Vertical(i,3))/size(d,2):Vertical(i,4);
        zv=zeros(size(d));
        sk=sk*krok/1000;
        for k=1:size(d,2)
            zv(k)=sum(sk(1:k));
        end
    end
    %Zmenšování sklonu
    if(Vertical(i,1)==2 && abs(Vertical(i,3))>abs(Vertical(i,4)))
        d=krok:krok:Vertical(i,2);
        sk=fliplr(Vertical(i,4)+(Vertical(i,3)-Vertical(i,4))/size(d,2):(Vertical(i,3)-Vertical(i,4))/size(d,2):Vertical(i,3));
        zv=zeros(size(d));
        sk=sk*krok/1000;
        for k=1:size(d,2)
            zv(k)=sum(sk(1:k));
        end
    end
    
    
    if(i==1)
        ZV=[0 zv];
    else
        zv=ZV(size(ZV,2))+zv;
        ZV=[ZV zv];
    end
end

%************************************
%Pøiøazení polomìru a úhlu natoèení kladek a zmìny rozchodu ke každému bodu
%trati

for i=1:j
    %Pøímý úsek
    if(Horizontal(i,1)==1)
        d=krok:krok:Horizontal(i,2);
        r=zeros(size(d));
        fi1=r;
        fi2=r;
        fi3=r;
        fi4=r;
        du=r;
    end
    %Oblouk
    if(Horizontal(i,1)==2)
        d=krok:krok:Horizontal(i,2);
        m=size(d);
        r=1/Horizontal(i,3)*ones(m);
        if(Horizontal(i,3)>0)
            fi1=asin(dp/(2*Horizontal(i,3)-ss-du(1)))*ones(m);
            fi2=asin(dp/(2*Horizontal(i,3)+ss+du(1))*ones(m));
            fi3=asin(-dp/(2*Horizontal(i,3)-ss-du(1)))*ones(m);
            fi4=asin(-dp/(2*Horizontal(i,3)+ss+du(1)))*ones(m);
        else
            fi1=asin(dp/(2*Horizontal(i,3)-ss-du(1)))*ones(m);
            fi2=asin(dp/(2*Horizontal(i,3)+ss+du(1)))*ones(m);
            fi3=asin(-dp/(2*Horizontal(i,3)-ss-du(1)))*ones(m);
            fi4=asin(-dp/(2*Horizontal(i,3)+ss+du(1)))*ones(m);
        end
    end
    %Pøechodnice
    if(Horizontal(i,1)==3)
        d=krok:krok:Horizontal(i,2);
        m=size(d,2);
        if(abs(Horizontal(i,3))<abs(Horizontal(i,4)))
            r=d./(Horizontal(i,2)*Horizontal(i,4));
            if(Horizontal(i,4)>0)
                fix=asin(dp/(2*Horizontal(i,4)-ss-dy));
                fi1=fix/m:fix/m:fix;
                fix=asin(dp/(2*Horizontal(i,4)+ss+dy));
                fi2=fix/m:fix/m:fix;
                fix=asin(-dp/(2*Horizontal(i,4)-ss-dy));
                fi3=fix/m:fix/m:fix;
                fix=asin(-dp/(2*Horizontal(i,4)+ss+dy));
                fi4=fix/m:fix/m:fix;
            else
                fix=asin(dp/(2*Horizontal(i,4)-ss-dy));
                fi1=fix/m:fix/m:fix;
                fix=asin(dp/(2*Horizontal(i,4)+ss+dy));
                fi2=fix/m:fix/m:fix;
                fix=asin(-dp/(2*Horizontal(i,4)-ss-dy));
                fi3=fix/m:fix/m:fix;
                fix=asin(-dp/(2*Horizontal(i,4)+ss+dy));
                fi4=fix/m:fix/m:fix;
            end
            
            
        elseif(abs(Horizontal(i,4))<abs(Horizontal(i,3)))
            r=fliplr(d./((Horizontal(i,2)*Horizontal(i,3))));
            if(Horizontal(i,3)>0)
                fix=asin(dp/(2*Horizontal(i,3)-ss-dy));
                fi1=fliplr(fix/m:fix/m:fix);
                fix=asin(dp/(2*Horizontal(i,3)+ss+dy));
                fi2=fliplr(fix/m:fix/m:fix);
                fix=asin(-dp/(2*Horizontal(i,3)-ss-dy));
                fi3=fliplr(fix/m:fix/m:fix);
                fix=asin(-dp/(2*Horizontal(i,3)+ss+dy));
                fi4=fliplr(fix/m:fix/m:fix);
            else
                fix=asin(dp/(2*Horizontal(i,3)-ss-dy));
                fi1=fliplr(fix/m:fix/m:fix);
                fix=asin(dp/(2*Horizontal(i,3)+ss+dy));
                fi2=fliplr(fix/m:fix/m:fix);
                fix=asin(-dp/(2*Horizontal(i,3)-ss-dy));
                fi3=fliplr(fix/m:fix/m:fix);
                fix=asin(-dp/(2*Horizontal(i,3)+ss+dy));
                fi4=fliplr(fix/m:fix/m:fix);
            end
            
        end
    end
    if(i==1)
        D=[0 d];
        R=[r(1) r];
        FI1p=[fi1(1) fi1];
        FI2p=[fi2(1) fi2];
        FI3p=[fi3(1) fi3];
        FI4p=[fi4(1) fi4];
    else
        d=d+D(size(D,2));
        R=[R,r];
        D=[D,d];
        FI1p=[FI1p fi1];
        FI2p=[FI2p fi2];
        FI3p=[FI3p fi3];
        FI4p=[FI4p fi4];
    end
end

m=size(D,2);

%************************************
%Pøiøazení výšky kladek dle sklonu trati ke každému bodu trati

j=size(Superel,1);

for i=1:j
    if(Superel(i,1)==1)
        d=krok:krok:Superel(i,2);
        zs=Superel(i,3)*ones(size(d));
    elseif(Superel(i,1)==2)
        if(Superel(i,3)<=Superel(i,4))
            d=krok:krok:Superel(i,2);
            n=size(d,2);
            zs=Superel(i,4)/n:(Superel(i,4)-Superel(i,3))/n:Superel(i,4);
        elseif(Superel(i,3)>Superel(i,4))
            d=krok:krok:Superel(i,2);
            n=size(d,2);
            zs=Superel(i,3):(Superel(i,4)-Superel(i,3))/n:Superel(i,3)/n;
        end
    end
    if(i==1)
        ZS=[zs(1) zs];
    else
        ZS=[ZS zs];
    end
end

%************************************
%Pøiøazení zmìny rozchodu ke každému bodu trati

j=size(ZmenaRozchodu,1);

for i=1:j
    if(ZmenaRozchodu(i,1)==1)
        d=krok:krok:ZmenaRozchodu(i,2);
        du=ZmenaRozchodu(i,3)*ones(size(d));
    elseif(ZmenaRozchodu(i,1)==2)
        if(ZmenaRozchodu(i,3)<=ZmenaRozchodu(i,4))
            d=krok:krok:ZmenaRozchodu(i,2);
            n=size(d,2);
            du=ZmenaRozchodu(i,4)/n:(ZmenaRozchodu(i,4)-ZmenaRozchodu(i,3))/n:ZmenaRozchodu(i,4);
        elseif(ZmenaRozchodu(i,3)>ZmenaRozchodu(i,4))
            d=krok:krok:ZmenaRozchodu(i,2);
            n=size(d,2);
            du=ZmenaRozchodu(i,3):(ZmenaRozchodu(i,4)-ZmenaRozchodu(i,3))/n:ZmenaRozchodu(i,3)/n;
        end
    end
    if(i==1)
        DU=[du(1) du];
    else
        DU=[DU du];
    end
end

DU=DU/1000;

%*************************************
%Naètení rychlostního profilu ze souboru

A=importdata(uigetfile('*.*'),'');
k=size(A,1);
Speed=zeros(1,4);
l=1;

for i=1:k
    h=char(A(i));
    if(size(h,2)>4)

        if(strcmp(h(4),'S')==1)
            g=strsplit(h);
            if(strcmp(h(9:11),'CON')==1)
                Speed(l,1)=1;
                Speed(l,2)=str2double(char(g(4)));
                Speed(l,3)=str2double(char(g(5)));
            end
            if(strcmp(h(9:11),'LIN')==1)
                Speed(l,1)=2;
                Speed(l,2)=str2double(char(g(4)));
                Speed(l,3)=str2double(char(g(5)));
                Speed(l,4)=str2double(char(g(6)));
            end
            l=l+1;
        end
        if(strcmp(h(1),'S')==1)
            g=strsplit(h);
            if(strcmp(h(6:8),'CON')==1)
                Speed(l,1)=1;
                Speed(l,2)=str2double(char(g(3)));
                Speed(l,3)=str2double(char(g(4)));
            end
            if(strcmp(h(6:8),'LIN')==1)
                Speed(l,1)=2;
                Speed(l,2)=str2double(char(g(3)));
                Speed(l,3)=str2double(char(g(4)));
                Speed(l,4)=str2double(char(g(5)));
            end
            l=l+1;
        end
    end
end

clear A

%*************************************
%Pøiøazení rychlosti k jednotlivým bodùm trati

l=size(Speed,1);

for i=1:l
    d=krok:krok:Speed(i,2);
    if(Speed(i,1)==1)
        v=Speed(i,3)*ones(size(d));
    elseif(Speed(i,1)==2)
        v=Speed(i,3)+(Speed(i,4)-Speed(i,3))/size(d,2):(Speed(i,4)-Speed(i,3))/size(d,2):Speed(i,4);
    end
    
    if(i==1)
        V=[v(1) v];
    else
        V=[V v];
    end
end

%************************************
%Pøepoèet výšky kladek dle sklonu trati na výšku vyvolávající požadované 
%pøíèné (odstøedivé) síly

G=9.81;
Fod=V.^2.*abs(R);
Fv=sqrt(G^2+Fod.^2);
beta=atan(Fod./G);
alfa=atan(ZS./ss);
gama=beta-alfa;
Fp=Fv.*sin(gama);
alfa2=acos(Fp/G);

if(isreal(alfa2)==1)
    ZScor=-(ss)./tan(alfa2);
else
    disp('Odstrediva sila je prilis velka. Nelze ji nahradit tihovou silou podvozku a opacnym sklonem.');
    ZScor=zeros(1,m);
end

%*************************************
%Výpoèet èasu pro jednotlivé body trati

T=zeros(1,m);

for i=1:m-1
    vs=(V(i)+V(i+1))/2;
    T(i+1)=T(i)+krok/vs;
end

%************************************
%Otáèky kladek a pøiøazení sklonu trati ZS a ZScor a zmìny rozchodu DU na 
%kolejnicové pásy

N1=zeros(1,m);
N2=N1;
ZSp1=N1;
ZSp2=N1;
ZScorp1=N1;
ZScorp2=N1;

for i=1:m
    %Pøímý úsek
    if(R(i)==0)
        N1(i)=60*V(i)/dk/pi;
        N2(i)=N1(i);
        
    elseif(R(i)>0)
        N1(i)=60*V(i)*(R(i)^(-1)-ss/2-DU(i)/2)/dk/pi/R(i)^(-1);
        N2(i)=60*V(i)*(R(i)^(-1)+ss/2+DU(i)/2)/dk/pi/R(i)^(-1);
        ZSp2(i)=ZS(i)/2;
        ZSp1(i)=-ZS(i)/2;
        ZScorp2(i)=ZScor(i)/2;
        ZScorp1(i)=-ZScor(i)/2;
        
    elseif(R(i)<0)
        N2(i)=60*V(i)*(-R(i)^(-1)-ss/2-DU(i)/2)/dk/pi/-R(i)^(-1);
        N1(i)=60*V(i)*(-R(i)^(-1)+ss/2+DU(i)/2)/dk/pi/-R(i)^(-1);
        ZSp1(i)=ZS(i)/2;
        ZSp2(i)=-ZS(i)/2;
        ZScorp1(i)=ZScor(i)/2;
        ZScorp2(i)=-ZScor(i)/2;
    end
end

N3=N1;
N4=N2;
ZSp3=ZSp1;
ZSp4=ZSp2;
ZScorp3=ZScorp1;
ZScorp4=ZScorp2;
DUp1=DU/2;
DUp2=-DU/2;
DUp3=DUp1;
DUp4=DUp2;

%************************************
%Pøedsunutí lichých a zpoždìní sudých kladek (natoèení, posuny ZV, ZS, 
%ZScor, rozšíøení DU)

n=round(dp/2/krok);
FI1=zeros(size(FI1p));
FI2=FI1;
FI3=FI1;
FI4=FI1;
ZVp1=FI1;
ZVp2=FI1;
ZVp3=FI1;
ZVp4=FI1;
ZS1=FI1;
ZS2=FI1;
ZS3=FI1;
ZS4=FI1;
ZScor1=FI1;
ZScor2=FI1;
ZScor3=FI1;
ZScor4=FI1;
DU1=FI1;
DU2=FI1;
DU3=FI1;
DU4=FI1;
prvni=1;
Rr=R;
Rr(m+1)=R(m);


for i=1:m
        if(Rr(i)==0)
            prvni=1;
            if((i+n)<m)
                FI1(i)=FI1p(i+n);
                FI2(i)=FI2p(i+n);
                ZVp1(i)=ZV(i+n);
                ZVp2(i)=ZV(i+n);
                ZS1(i)=ZSp1(i+n);
                ZS2(i)=ZSp2(i+n);
                ZScor1(i)=ZScorp1(i+n);
                ZScor2(i)=ZScorp2(i+n);
                DU1(i)=DUp1(i+n);
                DU2(i)=DUp2(i+n);
            else
                FI1(i)=FI1p(m);
                FI2(i)=FI2p(m);
                ZVp1(i)=ZV(m);
                ZVp2(i)=ZV(m);
                ZS1(i)=ZSp1(m);
                ZS2(i)=ZSp2(m);
                ZScor1(i)=ZScorp1(m);
                ZScor2(i)=ZScorp2(m);
                DU1(i)=DUp1(m);
                DU2(i)=DUp2(m);
            end
            if((i-n)>0)
                FI3(i)=FI3p(i-n);
                FI4(i)=FI4p(i-n);
                ZVp3(i)=ZV(i-n);
                ZVp4(i)=ZV(i-n);
                ZS3(i)=ZSp3(i-n);
                ZS4(i)=ZSp4(i-n);
                ZScor3(i)=ZScorp3(i-n);
                ZScor4(i)=ZScorp4(i-n);
                DU3(i)=DUp3(i-n);
                DU4(i)=DUp4(i-n);
            else
                FI3(i)=FI3p(1);
                FI4(i)=FI4p(1);
                ZVp3(i)=ZV(1);
                ZVp4(i)=ZV(1);
                ZS3(i)=ZSp3(1);
                ZS4(i)=ZSp4(1);
                ZScor3(i)=ZScorp3(1);
                ZScor4(i)=ZScorp4(1);
                DU3(i)=DUp3(1);
                DU4(i)=DUp4(1);
            end
        elseif(Rr(i+1)==Rr(i) && R(i)~=0)
            prvni=1;
            l1=round(R(i)^(-1)*FI1p(i)/krok);
            l2=round(R(i)^(-1)*FI2p(i)/krok);
            l3=round(R(i)^(-1)*FI3p(i)/krok);
            l4=round(R(i)^(-1)*FI4p(i)/krok);
                          
            if((i+l1)>=m)l1=m-i;end
            if((i+l1)<1)l1=1-i;end
                FI1(i)=FI1p(i+l1);
                ZVp1(i)=ZV(i+l1);
                ZS1(i)=ZSp1(i+l1);
                ZScor1(i)=ZScorp1(i+l1);
                DU1(i)=DUp1(i+l1);
                
            if((i+l2)>=m)l2=m-i;end
            if((i+l2)<1)l2=1-i;end
                FI2(i)=FI2p(i+l2);
                ZVp2(i)=ZV(i+l2);
                ZS2(i)=ZSp2(i+l2);
                ZScor2(i)=ZScorp2(i+l2);
                DU2(i)=DUp2(i+l2);
            
            if((i+l3)>=m)l3=m-i;end
            if((i+l3)<1)l3=1-i;end
                FI3(i)=FI3p(i+l3);
                ZVp3(i)=ZV(i+l3);
                ZS3(i)=ZSp3(i+l3);
                ZScor3(i)=ZScorp3(i+l3);
                DU3(i)=DUp3(i+l3);
            
            if((i+l4)>=m)l4=m-i;end
            if((i+l4)<1)l4=1-i;end
                FI4(i)=FI4p(i+l4);
                ZVp4(i)=ZV(i+l4);
                ZS4(i)=ZSp4(i+l4);
                ZScor4(i)=ZScorp4(i+l4);
                DU4(i)=DUp4(i+l4);
            
        elseif(abs(Rr(i))<=abs(Rr(i+1)) && R(i)~=0)
            if(prvni==1)
                l=i;
                while(Rr(l)~=Rr(l+1))
                    l=l+1;
                    if(l==m)break;end
                end
                lr=l-i;
                l0=n;
                prvni=0;
                l1=round(R(l)^(-1)*FI1p(l)/krok);
                l2=round(R(l)^(-1)*FI2p(l)/krok);
                l3=round(R(l)^(-1)*FI3p(l)/krok);
                l4=round(R(l)^(-1)*FI4p(l)/krok);
            end
      
                ip=round(l0+(l1-l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI1(i)=FI1p(i+ip);
                ZVp1(i)=ZV(i+ip);
                ZS1(i)=ZSp1(i+ip);
                ZScor1(i)=ZScorp1(i+ip);
                DU1(i)=DUp1(i+ip);
                         
                ip=round(l0+(l2-l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI2(i)=FI2p(i+ip);
                ZVp2(i)=ZV(i+ip);
                ZS2(i)=ZSp2(i+ip);
                ZScor2(i)=ZScorp2(i+ip);
                DU2(i)=DUp2(i+ip);

                ip=round(-l0+(l3+l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-l;end
                if((i+ip)<1)ip=1-i;end
                FI3(i)=FI3p(i+ip);
                ZVp3(i)=ZV(i+ip);
                ZS3(i)=ZSp3(i+ip);
                ZScor3(i)=ZScorp3(i+ip);
                DU3(i)=DUp3(i+ip);

                ip=round(-l0+(l4+l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-l;end
                if((i+ip)<1)ip=1-i;end
                FI4(i)=FI4p(i+ip);
                ZVp4(i)=ZV(i+ip);
                ZS4(i)=ZSp4(i+ip);
                ZScor4(i)=ZScorp4(i+ip);
                DU4(i)=DUp4(i+ip);

        elseif(abs(Rr(i))>=abs(Rr(i+1)) && R(i)~=0)
            if(prvni==1)
                l=i;
                l1=round(R(l)^(-1)*FI1p(l)/krok);
                l2=round(R(l)^(-1)*FI2p(l)/krok);
                l3=round(R(l)^(-1)*FI3p(l)/krok);
                l4=round(R(l)^(-1)*FI4p(l)/krok);
                l=i;
                while(Rr(l)~=Rr(l+1))
                    l=l+1;
                end
                lr=l-i;
                l0=n;
                prvni=0;
            end

                ip=round(l0+(l1-l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI1(i)=FI1p(i+ip);
                ZVp1(i)=ZV(i+ip);
                ZS1(i)=ZSp1(i+ip);
                ZScor1(i)=ZScorp1(i+ip);
                DU1(i)=DUp1(i+ip);

                ip=round(l0+(l2-l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI2(i)=FI2p(i+ip);
                ZVp2(i)=ZV(i+ip);
                ZS2(i)=ZSp2(i+ip);
                ZScor2(i)=ZScorp2(i+ip);
                DU2(i)=DUp2(i+ip);

                ip=round(-l0+(l3+l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI3(i)=FI3p(i+ip);
                ZVp3(i)=ZV(i+ip);
                ZS3(i)=ZSp3(i+ip);
                ZScor3(i)=ZScorp3(i+ip);
                DU3(i)=DUp3(i+ip);

                ip=round(-l0+(l4+l0)/lr*(i-l+lr));
                if((i+ip)>=m)ip=m-i;end
                if((i+ip)<1)ip=1-i;end
                FI4(i)=FI4p(i+ip);
                ZVp4(i)=ZV(i+ip);
                ZS4(i)=ZSp4(i+ip);
                ZScor4(i)=ZScorp4(i+ip);
                DU4(i)=DUp4(i+ip);
            
        end
end

clear Rr

%************************************
%Korekce ve smìru X v závislosti na posunu ve smìru Z zpùsobeného pøevýšení

ZV1=ZVp1-ZVp3;
ZV2=ZVp2-ZVp4;
% X1=dp-sqrt(dp^2-ZV1.^2);
% X2=dp-sqrt(dp^2-ZV2.^2);

%************************************
%Posun ve smìru Y

Y1=DU1;
Y2=DU2;
Y3=DU3;
Y4=DU4;

%************************************
%Souèet posunù kladek ve smìru Z

Z1=ZV1+ZS1;
Z2=ZV2+ZS2;
Z3=ZS3;
Z4=ZS4;

Zcor1=ZV1+ZScor1;
Zcor2=ZV2+ZScor2;
Zcor3=ZScor3;
Zcor4=ZScor4;
                    


%************************************
%Vykreslení vypoètených hodnot do grafù

%Vykreslení trati do roviny XY
figure(1)
grid on; hold on; axis equal;
plot(XYZ(1,:),XYZ(2,:));
title('Vykresleni trati v rovine XY');
xlabel('X [m]');
ylabel('Y [m]');

%Natoèení kladek v závislosti na dráze a èase
figure(2)
subplot(121)
plot(D,FI1,D,FI2,D,FI3,D,FI4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Natoceni kladek v závislosti na draze');
xlabel('D [m]');
ylabel('\phi [rad]');

subplot(122)
plot(T,FI1,T,FI2,T,FI3,T,FI4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Natoceni kladek v závislosti na case');
xlabel('t [s]');
ylabel('\phi [rad]');

%Otáèky kladek v závislosti na dráze a èase
figure(3)
subplot(121)
plot(D,N1,D,N2,D,N3,D,N4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Otacky kladek v závislosti na draze');
xlabel('D [m]');
ylabel('n [min^-^1]');

subplot(122)
plot(T,N1,T,N2,T,N3,T,N4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Otacky kladek v závislosti na case');
xlabel('t [s]');
ylabel('n [min^-^1]')

%Posunutí ve smìru Z bez korekce pro pøíènou sílu v závislosti na dráze a èase
figure(4)
subplot(121)
plot(D,Z1,D,Z2,D,Z3,D,Z4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Z bez korekce kladek v závislosti na draze');
xlabel('D [m]');
ylabel('Z [m]');

subplot(122)
plot(T,Z1,T,Z2,T,Z3,T,Z4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Z bez korekce kladek v závislosti na case');
xlabel('t [s]');
ylabel('Z [m]');

%Posunutí ve smìru Z s korekcí pro pøíènou sílu v závislosti na dráze a èase
figure(5)
subplot(121)
plot(D,Zcor1,D,Zcor2,D,Zcor3,D,Zcor4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Z s korekci kladek v závislosti na draze');
xlabel('D [m]');
ylabel('Z [m]');

subplot(122)
plot(T,Zcor1,T,Zcor2,T,Zcor3,T,Zcor4);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Z s korekci kladek v závislosti na case');
xlabel('t [s]');
ylabel('Z [m]');

%Posunutí kladek ve smìru Y z dùvodu zmìny rozchodu v závislosti na dráze a èase
figure(6)
subplot(121)
plot(D,Y1*1000,D,Y2*1000,D,Y3*1000,D,Y4*1000);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Y kladek v závislosti na draze');
xlabel('D [m]');
ylabel('Y [mm]');

subplot(122)
plot(T,Y1*1000,T,Y2*1000,T,Y3*1000,T,Y4*1000);
grid on
legend('Kladka 1','Kladka 2','Kladka 3','Kladka 4');
title('Posunuti Y kladek v závislosti na draze');
xlabel('t [s]');
ylabel('Y [mm]');


%************************************
%Uložení vypoètených dat

C=input('Ulozit vypoctena data "y"/"n": ','s');
if isempty(C)
    C = 'y';
end
if(C=='y')
    CC=input('Zadejte nazev souboru: ','s');
    save(CC,'D','R','DU','V','ZS','ZV','T','FI1','FI2','FI3','FI4','N1','N2','N3','N4','Z1','Z2','Z3','Z4','Zcor1','Zcor2','Zcor3','Zcor4','Y1','Y2','Y3','Y4');
end