%% sym -> GET JACOBIAN of integrated system
addpath ./lib
clc
clear all
close all

%------------------------------Manipulator--------------------
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

syms teta1 teta2 teta3 teta4 teta5 teta6 real

% teta1= sym('teta1');
% teta2= sym('teta2');
% teta3= sym('teta3');
% teta4= sym('teta4');
% teta5= sym('teta5');
% teta6= sym('teta6');

DH=[teta1 a1 0 pi/2
    teta2+pi/2 a2 0 0
    teta3 a3 0 pi/2
    teta4 0 d4 -pi/2
    teta5 0 0 pi/2
    teta6 0 d6 0];


[AA] = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end

%Transformada plat_base to end effector
pb_T_ee=T_ee;

X_b1=pb_T_ee(1,4);%postions..
Y_b1=pb_T_ee(2,4);
Z_b1=pb_T_ee(3,4);
%angles.. phi, teta, psi 
[A_b1, B_b1, C_b1]=invRPY(pb_T_ee);

%---------------------Platform------------------------------
% X_p= sym('X_p');
% Y_p= sym('Y_p');
% teta_p= sym('teta_p');


syms X_p Y_p teta_p real

z_platform= 0; 

%-----------------------------------------------------------
%=====================Sistema total=========================
%posição do end effector com influencia da platform
X_ee= X_p + X_b1*cos(teta_p) - Y_b1*sin(teta_p);
Y_ee= Y_p + X_b1*sin(teta_p) + Y_b1*cos(teta_p);
Z_ee= Z_b1 + z_platform;
A_ee=teta_p + A_b1;
B_ee=B_b1;
C_ee=C_b1;

%----------------jacobiano------------------
J= jacobian([X_ee; Y_ee; Z_ee; A_ee; B_ee; C_ee], [X_p Y_p teta_p teta1 teta2 teta3 teta4 teta5 teta6]);

J=simplify(J);

save('jacobiano_mat_aprox1.mat','J');

%g = matlabFunction(J,'vars',{'X_p','Y_p','teta_p','teta1','teta2','teta3','teta4','teta5','teta6'});

%save('jacobiano_mat.mat','J');

%J_simplified=simplify(J)

%% 
% clc
% clear all
% J4=[teta1+teta2 teta3
%     teta4 teta3+teta4];
% 
% fid = fopen('Mymatrix.txt','wt');
% for ii = 1:size(J4,1)
%     fprintf(fid,'%s',J4(ii,:));
%     fprintf(fid,'\n');
% end
% fclose(fid)






