function [C_out,V_out,t_out] = run(C_in, V_in, grav_loc, dt, n_iter, b_osy)
global C V grav

C = C_in;
V = V_in;
grav = grav_loc;
t_out = zeros(n_iter+1,1);

% uprava velicin C V
for i = 2:length(C)
    C{i}.xtisk = C{i}.xtisk - C{i}.x(1);
    C{i}.ytisk = C{i}.ytisk - C{i}.x(2);
    C{i}.xt = zeros(n_iter+1,3);
    C{i}.xt(1,:) = C{i}.x;
    C{i}.vt(1,:) = C{i}.v;
end

for i = 1:length(V)
    switch(V{i}.typ)
        case 1 % rotacni vazba
            V{i}.rA = V{i}.xr - C{V{i}.T1}.x(1:2);
            V{i}.rB = V{i}.xr - C{V{i}.T2}.x(1:2);
            
        case 2 % posuvna vazba
            V{i}.rA = V{i}.xr - C{V{i}.T1}.x(1:2);
            V{i}.rB = V{i}.xr - C{V{i}.T2}.x(1:2);
            
        case 3 % kinematicka vazba
            V{i}.T2 = 1;
    end
    V{i}.R = zeros(n_iter+1,6); % reakce ve vazbe
end

% nastaveni pocatecnich podminek
nb = length(C)-1;
U = zeros(6*nb,1);
K = 1:6;
for i = 1:nb
    U(6*(i-1)+K) = [C{i+1}.x; C{i+1}.v];
end

figure('color','w')
hold on;
% tisk teles
for i = 1:nb
    x = C{i+1}.x(1) + C{i+1}.xtisk;
    y = C{i+1}.x(2) + C{i+1}.ytisk;
    C{i+1}.tisk = plot(x, y, 'linewidth', 3, 'color', 'r');
end

% tisk vazeb
marker = ['o','x'];
for i = 1:length(V)
    if(V{i}.typ ~= 3)
        T1 = V{i}.T1;
        rA = V{i}.rA;
        x = C{T1}.x(1) + rA(1)*cos(C{T1}.x(3)) - rA(2)*sin(C{T1}.x(3));
        y = C{T1}.x(2) + rA(1)*sin(C{T1}.x(3)) + rA(2)*cos(C{T1}.x(3));
        V{i}.tisk = plot(x,y,'marker',marker(V{i}.typ),'markersize',10, 'linewidth', 2);
    end
end
axis equal
% nastaveni os
osy = 1000*[1 -1 1 -1];
for i = 1:nb
    osy(1) = min([osy(1),C{i+1}.xtisk + C{i+1}.x(1)]);
    osy(2) = max([osy(2),C{i+1}.xtisk + C{i+1}.x(1)]);
    osy(3) = min([osy(3),C{i+1}.ytisk + C{i+1}.x(2)]);
    osy(4) = max([osy(4),C{i+1}.ytisk + C{i+1}.x(2)]);
end
osy = osy + b_osy*[-1 1 -1 1];
axis(osy)
box on;
grid on;

t = 0;
for iter = 1:n_iter
   
    % vlastni integrace pohybovych rovnic (Rungeova-Kuttova metoda 4. řádu přesnosti)
    K1 = f(U,t);
    K2 = f(U+dt/2*K1,t);
    K3 = f(U+dt/2*K2,t);
    K4 = f(U+dt*K3,t);
    U = U + dt/6*(K1+2*K2+2*K3+K4);
    
    t = t + dt;
    t_out(iter+1) = t;
    
    % aktualizace polohy
    for i = 1:nb
        Ix = (i-1)*6 + (1:3);
        Iv = (i-1)*6 + (4:6);
        
        C{i+1}.x = U(Ix);
        C{i+1}.v = U(Iv);
        
        C{i+1}.xt(iter+1,:) = C{i+1}.x;
        C{i+1}.vt(iter+1,:) = C{i+1}.v;
    end
    
    % ulozeni reakcnich vazeb
    for i = 1:length(V)
        V{i}.R(iter+1,:) = V{i}.R_loc;
    end
    
    % tisk mechanismu
    if(mod(iter,2) == 0)
        for i = 1:nb
            x = C{i+1}.x(1) + C{i+1}.xtisk*cos(C{i+1}.x(3)) - C{i+1}.ytisk*sin(C{i+1}.x(3));
            y = C{i+1}.x(2) + C{i+1}.xtisk*sin(C{i+1}.x(3)) + C{i+1}.ytisk*cos(C{i+1}.x(3));
            set(C{i+1}.tisk,'Xdata',x,'Ydata',y);
        end

        % tisk vazeb
        for i = 1:length(V)
            if(V{i}.typ ~= 3)
                T1 = V{i}.T1;
                rA = V{i}.rA;
                x = C{T1}.x(1) + rA(1)*cos(C{T1}.x(3)) - rA(2)*sin(C{T1}.x(3));
                y = C{T1}.x(2) + rA(1)*sin(C{T1}.x(3)) + rA(2)*cos(C{T1}.x(3));
                set(V{i}.tisk,'Xdata',x,'Ydata',y);
            end
        end
        title([num2str(t,'%5.2f'),'s']);
        drawnow
    end
end
C_out = C;
V_out = V;


%__________________________________________________________________________
function K = f(U,t)
global C V grav
    
    % aktualizace polohy
    nb = length(C)-1;
    for i = 1:nb
        Ix = (i-1)*6 + (1:3);
        Iv = (i-1)*6 + (4:6);
        C{i+1}.x = U(Ix);
        C{i+1}.v = U(Iv);
    end

    Ih = zeros(20*nb,1);
    Jh = zeros(20*nb,1);
    Hh = zeros(20*nb,1);
    b = zeros(20*nb,1);
    
    % plneni dynamicke casti matice A
    s = 1;
    for k = 1:nb
        for i = 1:3
            ind = (k-1)*6 + i;
            Ih(s) = ind;
            Jh(s) = ind;
            Hh(s) = 1;
            s = s + 1;
            
            b(ind) = U(ind + 3);
        end
         for i = 1:3
            ind = (k-1)*6 + i + 3;
            Ih(s) = ind;
            Jh(s) = ind;
            Hh(s) = C{k+1}.M(i);
            s = s + 1;
            
            % pricteni gravitacnich a vnejsich sil
            b(ind) = C{k+1}.M(i)*grav(i) + C{k+1}.Q(i);
         end
    end
    
    % plneni Lagrangeovych multiplikatoru
    nv = length(V);
    for k = 1:nv
        switch(V{k}.typ)
            case 1 % rotacni vazba
                T1 = V{k}.T1;
                T2 = V{k}.T2;
                [dF,RHS] = rotacni_vazba(V{k}.rA,C{T1}.x,C{T1}.v,V{k}.rB,C{T2}.x,C{T2}.v);
                V{k}.dF = dF;
            case 2 % posuvna vazba
                T1 = V{k}.T1;
                T2 = V{k}.T2;
                [dF,RHS] = posuvna_vazba(V{k}.rA,C{T1}.x,C{T1}.v,V{k}.rB,C{T2}.x,C{T2}.v,V{k}.n);
                V{k}.dF = dF;
            case 3 % kinematicka vazba
                T1 = V{k}.T1;
                [dF,RHS] = kinematicka_vazba(C{T1}.x,C{T1}.v,V{k}.alfa,t);
                V{k}.dF = dF;
        end
        
        % plneni globalni matice
        dF1 = dF(1:3,:);
        dF2 = dF(4:6,:);
        nmax = max(Ih);
        [p,q] = size(dF);
        for i = 1:3
            for j = 1:q
                if(V{k}.T1 ~= 1)
                    Ih(s) = (V{k}.T1-2)*6 + i + 3;
                    Jh(s) = nmax + j;
                    Hh(s) = dF1(i,j);
                    s = s + 1;

                    Ih(s) = nmax + j;
                    Jh(s) = (V{k}.T1-2)*6 + i + 3;
                    Hh(s) = dF1(i,j);
                    s = s + 1;
                end
                
                if(V{k}.T2 ~= 1)
                    Ih(s) = (V{k}.T2-2)*6 + i + 3;
                    Jh(s) = nmax + j;
                    Hh(s) = dF2(i,j);
                    s = s + 1;

                    Ih(s) = nmax + j;
                    Jh(s) = (V{k}.T2-2)*6 + i + 3;
                    Hh(s) = dF2(i,j);
                    s = s + 1;
                end
            end
        end
        
        for j = 1:q
            b(nmax+j) = RHS(j);
        end
    end

    ns = max(Ih);
    I = 1:s-1;
    A = sparse(Ih(I),Jh(I),Hh(I),ns,ns);
    b = b(1:ns);
    
    % reseni soustavy rovnic
    x = A\b;
    K = x(1:6*nb);
    
    % urceni reakci ve vazbach
    lambda = x(6*nb+1:end);
    s = 1;
    for k = 1:nv
        switch(V{k}.typ)
            case 1
                I = s:s+1;
                V{k}.R_loc = V{k}.dF*lambda(I);
                s = s + 2;
            case 2
                 I = s:s+1;
                V{k}.R_loc = V{k}.dF*lambda(I);
                s = s + 2;
            case 3
                I = s;
                V{k}.R_loc = V{k}.dF*lambda(I);
                s = s + 1;
        end
    end
    
    
    
function [dF,RHS] = rotacni_vazba(rA,x1,v1,rB,x2,v2)
% vazebni podminka
F = [x1(1)-x2(1) + rA(1)*cos(x1(3)) - rB(1)*cos(x2(3)) - rA(2)*sin(x1(3)) + rB(2)*sin(x2(3)); ...
     x1(2)-x2(2) + rA(1)*sin(x1(3)) - rB(1)*sin(x2(3)) + rA(2)*cos(x1(3)) - rB(2)*cos(x2(3))];

% derivace vazebni podminky
dF = [1, 0;
      0, 1;
      -rA(1)*sin(x1(3))-rA(2)*cos(x1(3)), rA(1)*cos(x1(3))-rA(2)*sin(x1(3));
      -1, 0;
      0, -1;
      rB(1)*sin(x2(3))+rB(2)*cos(x2(3)), -rB(1)*cos(x2(3))+rB(2)*sin(x2(3))];
  
gamma = [0;0];  
gamma(1) = (-rA(1)*cos(x1(3))+rA(2)*sin(x1(3)))*v1(3)^2 + (rB(1)*cos(x2(3))-rB(2)*sin(x2(3)))*v2(3)^2;
gamma(2) = (-rA(1)*sin(x1(3))-rA(2)*cos(x1(3)))*v1(3)^2 + (rB(1)*sin(x2(3))+rB(2)*cos(x2(3)))*v2(3)^2;

alfa = 1;
beta = 10;

tlum = -2*alfa*dF'*[v1;v2] - beta^2*F; % stabilizace
                      
RHS = -gamma + tlum;



function [dF,RHS] = posuvna_vazba(rA,x1,v1,rB,x2,v2,n)
nx = n(1);
ny = n(2);

% vazebni podminka
F = [x1(3)-x2(3); ...
    (x1(1)-x2(1))*(nx*cos(x1(3))-ny*sin(x1(3))) + (x1(2)-x2(2))*(nx*sin(x1(3))+ny*cos(x1(3))) + (rA(1)-rB(1))*nx + (rA(2)-rB(2))*ny];

% derivace vazebni podminky
dF = [0, nx*cos(x1(3))-ny*sin(x1(3));
      0, nx*sin(x1(3))+ny*cos(x1(3));
      1, (x1(1)-x2(1))*(-nx*sin(x1(3))-ny*cos(x1(3))) + (x1(2)-x2(2))*(nx*cos(x1(3))-ny*sin(x1(3)));
      0, -(nx*cos(x1(3))-ny*sin(x1(3)));
      0, -(nx*sin(x1(3))+ny*cos(x1(3)));
      -1, 0];
  
ddF = [0, 0, -nx*sin(x1(3))-ny*cos(x1(3)), 0, 0, 0;
       0, 0, nx*cos(x1(3))-ny*sin(x1(3)), 0, 0, 0;
       -nx*sin(x1(3))-ny*cos(x1(3)),...
       nx*cos(x1(3))-ny*sin(x1(3)),...
       (x1(1)-x2(1))*(-nx*cos(x1(3))+ny*sin(x1(3))) + (x1(2)-x2(2))*(-nx*sin(x1(3))-ny*cos(x1(3))), ...
       nx*sin(x1(3))+ny*cos(x1(3)),...
       -nx*cos(x1(3))+ny*sin(x1(3)), 0;
       0, 0, nx*sin(x1(3))-ny*cos(x1(3)), 0, 0, 0;
       0, 0, -nx*cos(x1(3))+ny*sin(x1(3)), 0, 0, 0;
       0, 0, 0, 0, 0, 0];
   
v = [v1;v2];
gamma = [0;0];  
gamma(2) = v'*ddF*v;

alfa = 10;
beta = 50;

tlum = -2*alfa*dF'*v - beta^2*F; % stabilizace
                      
RHS = -gamma + tlum;


function [dF,RHS] = kinematicka_vazba(x1,v1,alfa_str,t)
% vazebni podminka
alfa = eval(alfa_str);
F = x1(3)-alfa;

% derivace vazebni podminky
dF = [0 0 1 0 0 0]';

alfa = 1;
beta = 50;

tlum = -2*alfa*dF'*[v1;0;0;0] - beta^2*F; % stabilizace
                      
RHS = tlum;