%% Dane
pyp=1
pxp=1
pyk=2
pxk=-1
tk=10;
tp=0
L1=5.8;
L2=1;
VP=[-1;0]
VK=[0;1]

%% p na q
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]

%% Jakobiany
JP=[-L1*sin(q1p)-L2*sin(q1p+q2p) -L2*sin(q1p+q2p); L1*cos(q1p)+L2*cos(q1p+q2p) L2*cos(q1p+q2p)]

QPd=inv(JP)*VP
%% Wspolczynniki
a0=QP;
a1=QPd;
a2=3/tk^2*(QK-QP)
a3=-2/tk^3*(QK-QP)
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;
PX=L1*cos(y(1,1))+L2*cos(y(1,1)+y(2,1));
PY=L1*sin(y(1,1))+L2*sin(y(1,1)+y(2,1));
plot(PX,PY,'r.')
hold on
end