%Zadání promìnných
r = 1; % m
b = 0.2; % m
c = 1.9; % m
d = 0.3; % m
l = 0.6; % m
gama = 0.3; % rad/s
s = 0.1; % m
beta = 0.3; % rad
delta = 0.15; % rad
omega12 = 8; % 1/s
v23 = 1.8; % m/s
omega34 = 22; % 1/s

%Èasy - Pohyb bodù A,B, L bìhem jedné otáèky tìlesa 2 
T2 = 2*pi() / omega12;  %Jedna otáèka = jedna perioda
T = 2 * T2;   % Dvì otáèky =  ....... kolik asi

%Vektory vzdáleností 
r2A = [c*sin(gama); r + c * cos(gama); 1 ];
r3B = [0; 0; 1];
r4L = [l; 0; 1];
%Tenhle for cyklus je docela "prasárna", takže všem co umí programovat se
%omlouvám. Matlab má problém s vytvoøením matice obsahující další matice
i = 0;
for t = 0:0.01:T
    i = i+1;
    A = mr(fi12(t,beta,omega12))*r2A;
    B = mr(fi12(t,beta,omega12))*mp(0,r)*mr(-delta)*mp(0,s23(t,s,b,v23))*mp(d,0)*r3B;
    L = mr(fi12(t,beta,omega12))*mp(0,r)*mr(-delta)*mp(0,s23(t,s,b,v23))*mp(d,0)*mr(fi34(t,delta,omega34))*r4L;
    Ax(i) = A(1);
    Ay(i) = A(2);
    Bx(i) = B(1);
    By(i) = B(2);
    Lx(i) = L(1);
    Ly(i) = L(2);
end

%Pro jeden urèitý èas - teï jsem si vymyslel koneèný - dvì periody
t = 2*T;
At = mr(fi12(t,beta,omega12))*r2A;
Bt = mr(fi12(t,beta,omega12))*mp(0,r)*mr(-delta)*mp(0,s23(t,s,b,v23))*mp(d,0)*r3B;
Lt = mr(fi12(t,beta,omega12))*mp(0,r)*mr(-delta)*mp(0,s23(t,s,b,v23))*mp(d,0)*mr(fi34(t,delta,omega34))*r4L;
Axt = At(1);
Ayt = At(2);
Bxt = Bt(1);
Byt = Bt(2);
Lxt = Lt(1);
Lyt = Lt(2); 

%kontrola výpoètu vzdálenosti bodu B od bodu L, jsou na tyèi o délce l
%(0.6 m) a to se nemùže zmìnit
distance = sqrt((Bxt-Lxt)^2 + (Byt-Lyt)^2 );
fprintf('kontrola: l = %f = %f\n', distance, l)

plot(Ax,Ay,Bx,By,Lx,Ly,'g')
title('Trajektorie A,B,L')
legend('A','B','L')

%FUNKCE FUNKCE FUNKCE FUNKCE FUNKCE FUNKCE FUNKCE
function matice_posun = mp(x,y)
    matice_posun = [1 0 x; 0 1 y; 0 0 1];
end

function matice_rotace = mr(uhel)
    matice_rotace = [cos(uhel) -sin(uhel) 0; sin(uhel) cos(uhel) 0; 0 0 1];
end

function uhel12 = fi12(t, beta, omega12)
    uhel12 = pi()/2 - beta + omega12*t;
end

function dist23 = s23(t, s, b, v23)
    dist23 = s + b + v23 * t;
end

function uhel34 = fi34(t, delta, omega34)
    uhel34 = delta + omega34 * t;
end


