clc;
clear all;
close all;

% dados de entrada
C=[38.1 55.9 88.9]; 
q1=0.85; q2=0.25;
A=-0.105 ; Bl=124; % estimativas iniciais
q1p=-2.6; q2p=3.5;
q1pp=0.42; q2pp=0.68;

% 1o. passo - determinação de posições
q=[q1 q2]';
s=[A Bl];
s=mec4bard(C,q,s);
A=s(1);
Bl=s(2);

disp(['Para q1 = ',num2str(q1),' radianos e q2 = ',num2str(q2),...
      ' radianos, os valores desejados são'])
disp(' ')
disp(['A',' = ',num2str(A),' radianos;'])
disp(['B',' = ',num2str(Bl),' mm;'])
disp(' ')

% 2o. passo - determinação de velocidades
qp=[q1p q2p]';
J=jaco4bard(C,s);
B=[C(1)*sin(q1) -C(2)*sin(q2); -C(1)*cos(q1) C(2)*cos(q2)];
K=J\B;
sp=K*qp;
Ap=sp(1);
Bp=sp(2);
  
disp(['Para q1p = ',num2str(q1p),' rad/s e q2p = ',num2str(q2p),...
      ' rad/s, os valores desejados sao'])
disp(' ')
disp(['Ap',' = ',num2str(Ap),' rad/s;'])
disp(['Bp',' = ',num2str(Bp),' mm/s;'])
disp(' ')
 
% 3o. passo - determinação de acelerações
qpp=[q1pp q2pp]';
Bq1=[C(1)*cos(q1) 0; C(1)*sin(q1) 0];
Bq2=[0 -C(2)*cos(q2); 0 -C(2)*sin(q2)];
Jq1=[-Bl*cos(A) -sin(A); -Bl*sin(A) cos(A)]*[K(1,1) 0; K(2,1) K(1,1)];
Jq2=[-Bl*cos(A) -sin(A); -Bl*sin(A) cos(A)]*[K(1,2) 0; K(2,2) K(1,2)];
L1=J\(Bq1-Jq1*K);
L2=J\(Bq2-Jq2*K);
spp=K*qpp+q1p*L1*qp+q2p*L2*qp;
App=spp(1);
Bpp=spp(2);
 
disp(['Para q1pp = ',num2str(q1pp),' rad/s2 e q2pp = ',num2str(q2pp),...
      ' rad/s2, os valores desejados são'])
disp(' ')
disp(['App',' = ',num2str(App),' rad/s2;'])
disp(['Bpp',' = ',num2str(Bpp),' mm/s2;'])
disp(' ')