%% Dane
pyp=5
pxp=5
pyk=5
pxk=-10
tk=10;
tp=0
L1=5.8;
L2=1;

%% p na q (z odwrotnej)
q2p=acos((pyp^2+pxp^2-L1^2-L2^2)/(2*L1*L2))
q1p=asin((L2*sin(q2p))/(sqrt(pxp^2+pyp^2))+atan(pxp/pyp))
QP=[q1p;q2p]

q2k=acos((pyk^2+pxk^2-L1^2-L2^2)/(2*L1*L2))
q1k=asin((L2*sin(q2k))/(sqrt(pxk^2+pyk^2))+atan(pxk/pyk))
QK=[q1k;q2k]

%% Wspolczynniki
a0=QP;
a1=0;
a2=3*((QK-QP)/tk^2)
a3=-2*((QK-QP)/tk^3)
t=0:0.01:tk;
y=a0+a1*t+a2*t.^2+a3*t.^3;
figure(1)
plot(t,y);

%% Trajektoria
figure(2)
for t=0:0.01:tk
y=a0+a1*t+a2*t.^2+a3*t.^3;
PXD=L1*cos(y(1,1))+L2*cos(y(1,1)+y(2,1));
PYD=L1*sin(y(1,1))+L2*sin(y(1,1)+y(2,1));
plot(PXD,PYD,'bo')
hold on
end