%% 3 DOF-Spherical Manipulator Robot %%
syms L1 L2 L3
syms T1 T2 D3
syms pi

% Dynamic Symbols 
syms q1_dot q2_dot d3_dot % Generalized coordinates
syms m1 m2 m3             % Mass for each links 
syms g                    % Gravity
%% Forward_kinematics %%

A_1 = A_matrix(T1,L1,0,pi/2);
A_2 = A_matrix(T2+pi/2,0,0,pi/2);
A_3 = A_matrix(0,L2+D3,0,0);

A_01 = A_1;
A_02 = A_1*A_2;
A_03 = A_02*A_3;
fk = simplify((A_1*A_2)*A_3);
disp(fk)
%% Jacobian between coordinate 0 and coordinate 1 %% 
z_0 = [0 0 1]; 

O_0 = [0 0 0];
O_1 = [0 0 L1];

Jv_1 = cross(z_0, O_1-O_0);
Jw_1 = z_0;

Jv = Jv_1;
Jw = Jw_1;

J_01 = [Jv.'; Jw.';];
disp(J_01);

%% Jacobian between coordinate 0 and coordinate 2 %%
z_0 = [0 0 1];
z_1 = (A_1(1:3,3)).';

O_0 = [0 0 0];
O_1 = A_1(1:3,4).';
O_2 = A_02(1:3,4).';

Jv_1 = cross(z_0, O_2-O_0);
Jv_2 = cross(z_1, O_2-O_1);
Jw_1 = z_0;
Jw_2 = z_1;

Jv = [Jv_1; Jv_2;];
Jw = [Jw_1; Jw_2;];
J_02 = simplify([Jv.'; Jw.';]);
disp(J_02);

%% Jacobian between coordinate 0 and coordinate 3 %%
z_0 = [0; 0; 1;];
z_1 = simplify((A_1(1:3,3)));
z_2 = simplify((A_02(1:3,3)));

O_0 = [0; 0; 0;];
O_1 = A_1(1:3,4);
O_2 = A_02(1:3,4);
O_3 = A_03(1:3,4);

Jv_1 = cross(z_0, O_3-O_0);
Jv_2 = cross(z_1, O_3-O_1);
Jv_3 = z_2;
Jw_1 = z_0;
Jw_2 = z_1;
Jw_3 = [0; 0; 0;];

Jv = [Jv_1 Jv_2 Jv_3];
Jw = [Jw_1 Jw_2 Jw_3];
J_03 = simplify([Jv; Jw;]);
disp(J_03);

%% Inertia Tensors %% 
I_1 = [0 0 0;
       0 0 0;
       0 0 0;];

I_2 = [0 0 0;
       0 0 0;
       0 0 0;];

I_3 = [0 0 0;
       0 0 0;
       0 0 0;];

%% Kinetic Energy %%
% K = 1/2*q_dot.'(m*jv.'*jv + jw.'R*I*R.'*jw)*q_dot
% K = 1/2 m*v**2


R_01 = A_1(1:3,1:3);
R_02 = A_02(1:3,1:3);
R_03 = A_03(1:3,1:3);

Jv1 = J_01(1:3);
Jv2 = J_02(1:3,:);
Jv3 = J_03(1:3,:);

Jw1 = J_01(4:6);
Jw2 = J_02(4:6,:);
Jw3 = J_03(4:6,:);


v1 = Jv1*q1_dot;
v2 = Jv2*[q1_dot; q2_dot;];
v3 = Jv3*[q1_dot; q2_dot; d3_dot];

%w1 = ((Jw1).'*R_01*I_1*(R_01).'*Jw1)*q1_dot;
%w2 = ((Jw2).'*R_02*I_2*(R_02).'*Jw2)*[q1_dot; q2_dot;];

w1 = Jw1*q1_dot;
w2 = Jw2*[q1_dot; q2_dot;];
w3 = Jw3*[q1_dot; q2_dot; d3_dot;];

K_01 = (1/2)*m1*((v1).'*v1+(w1).'*(I_1)*w1);
K_02 = (1/2)*m2*((v2).'*v2+(w2).'*(I_2)*w2);
K_03 = simplify((1/2)*m3*((v3).'*v3+(w2).'*(I_3)*w3));

K = simplify(K_01+K_02+K_03);
disp(K)
%% Potential Energy %%
% P = mgh = mg*r_c

P_01 = m1*g*L1/2;
P_02 = m2*g*L1; 
P_03 = m3*g*(sin(T1)*L1 + sin(T2)*L2/2);
P = simplify(P_01+P_02+P_03);
disp(P);
%% Lagrangian %% 
%L = K-P
L = K-P; 
disp(L)
