function [DYDATA] = initialize_dynamic_data_6DOF()
%{
Function for deriving the equations of motion of the joint model with 6DOF.
--------------------------------------------------------------------------
Syntax :
[DYDATA] = initialize_dynamic_data_6DOF()
--------------------------------------------------------------------------
Function Description :

--------------------------------------------------------------------------
%}

% define the generalized coordinates
syms q1   q2     q3   q4  q5  q6 real;    %generalized coordinates
%    psi  theta  phi  x   y   z
syms dq1 dq2 dq3 dq4 dq5 dq6 real;        %generalized velocities
syms ddq1 ddq2 ddq3 ddq4 ddq5 ddq6 real;  %generalized accelerations

syms qg real; % generalized coordinate of the glenoid (scapula)

% and the generalized coordinates/velocities/accelerations vectors
q   = [q1; q2; q3; q4; q5; q6];
dq  = [dq1; dq2; dq3; dq4; dq5; dq6];
ddq = [ddq1; ddq2; ddq3; ddq4; ddq5; ddq6];

% define the constants
syms Ixx Iyy Izz M CG g real;

% inertia tensor around the center of mass
I = [Ixx 0 0; 0 Iyy 0; 0 0 Izz];

% define the rotation matrix for an Euler's sequence of YXY

R_psi = [cos(q1) 0 sin(q1); 
         0       1     0; 
         -sin(q1) 0  cos(q1)];

R_theta = [1     0         0;
           0  cos(q2)  -sin(q2);
           0  sin(q2)   cos(q2)];
  
R_phi = [cos(q3) 0 sin(q3); 
         0      1     0; 
         -sin(q3) 0  cos(q3)];

R_H2T = R_phi*R_theta*R_psi; % humerus to thorax (body-fixed to inertial)

R_G2T = [1     0         0;
         0  cos(qg)  -sin(qg);
         0  sin(qg)   cos(qg)]; % glenoid to thorax

% define the spatial configuration of the body (humerus) by the position
% of its center of mass in the inertial frame
r_HH = [q4 q5 q6]';                  % position vector of the humeral head
                                     % (tranlational vector) in thorax
OG = r_HH + R_H2T*[0 -CG 0]';

% angular velocity in the body-fixed frame:
Wrotation = dq3*R_psi'*R_theta'*[0 1 0]' + dq2*R_psi'*[1 0 0]' + dq1*[0 1 0]';

% angular velocity in the inertial frame:
Wrotation = simplify(expand(R_H2T*Wrotation));

% linear acceleration and velocity of the center of mass
V_CG = simplify(expand(jacobian(OG,q)*dq)); % linear velocity of the CG
A_CG = simplify(expand(jacobian(V_CG,q)*dq + jacobian(V_CG,dq)*ddq));

% Kinetic Energy
T = 0.5*M*[dq4 dq5 dq6]*[dq4 dq5 dq6]' + 0.5*Wrotation'*I*Wrotation;

% Potential energy
V = M*g*OG(2);

% Lagrangian
L = T - V;

% Derivations
MDYN = simplify(jacobian(jacobian(L,dq),dq));
RHS = simplify(-jacobian(jacobian(L,dq),q)*dq + jacobian(L,q)');

% save
[N1,N2]=size(MDYN);
N3=length(RHS);

fileID = fopen('EquationsofMotion_6DOF.m','w');
for i=1:N1
    for j=1:N2
        fprintf(fileID,'MDYN(%d,%d) = %s;\n', i, j, char(MDYN(i,j)));
    end
end
for i=1:N3
    fprintf(fileID,'RHS(%d,1) = %s;\n', i, char(RHS(i)));
end
fclose(fileID);

N4=length(A_CG);

fileID = fopen('LinearAccofCenterofMass_6DOF.m','w');
for i=1:N4
    fprintf(fileID,'A_CG(%d,1) = %s;\n', i, char(A_CG(i)));
end
fclose(fileID);

%{
% define the generalized forces
Dir_inT = sym('Dir_inT',[3 6], 'real');     % muscle-force direction
Rho_inT = sym('Rho_inT',[3 6], 'real');     % muscle leverarm
F_final = sym('F_final',[1 6], 'real');     % muscle-force magnitude


Dir_contact = sym('Dir_contact',[3 20], 'real'); % contact-force direction
Rho_contact = sym('Rho_contact',[3 20], 'real'); % contact-force leverarm
F_contact   = sym('F_contact',[1 20], 'real');   % contact-force magnitude

    V_HH = [dq4 dq5 dq6]';          % the velocity of HH
    F_resultant = zeros(3,1);       % initialize the resultant of forces
    M_resultant = zeros(3,1);       % initialize the resultant of moments
    
for j = 1:size(F_final,2) + size(F_contact,2) % the number of forces applying on the humerus
       
        if le(j,size(F_final,2))
            F_resultant = F_final(j)*Dir_inT(:,j) + F_resultant;
            M_resultant = cross(Rho_inT(:,j),F_final(j)*Dir_inT(:,j)) + M_resultant;
        else
            F_resultant = F_contact(j-size(F_final,2))*Dir_contact(:,j-size(F_final,2)) + F_resultant;
            M_resultant = cross(Rho_contact(:,j-size(F_final,2)),F_contact(j-size(F_final,2))*Dir_contact(:,j-size(F_final,2))) + M_resultant;
        end
end


for i = 1:6 % the number of generalized coordinates

        Q(i,1) = F_resultant'*jacobian(V_HH,dq(i)) + M_resultant'*jacobian(Wrotation,dq(i));
end

% perform some substitutions
for i=1:6
    str=char(Q(i));
    
    for j = 1:9 % the largest one digit number
        str = strrep(str, ['1' '_' num2str(j)], ['(' '1' ',' num2str(j) ')']);
        str = strrep(str, ['2' '_' num2str(j)], ['(' '2' ',' num2str(j) ')']);
        str = strrep(str, ['3' '_' num2str(j)], ['(' '3' ',' num2str(j) ')']);
        str = strrep(str, ['l' num2str(j)], ['l' '(' num2str(j) ')']);
        str = strrep(str, ['F_contact' num2str(j)], ['F_contact' '(' num2str(j) ')']);
    end
    for j = 0:9
        str = strrep(str, [')' num2str(j)], [num2str(j) ')']);
    end
    
    Q(i)=cellstr(str);
end

fileID = fopen('GeneralizedForces_6DOF.m','w');
for i=1:6
    fprintf(fileID,'Q(%d,1) = %s;\n',i,char(Q(i)));
end
fclose(fileID);

%}
% Defining the System Parameters

M   = 3.82;             %[Kg]
Ixx = 0.003+0.3914;     %[Kgm2]
Iyy = 0.024;            %[Kgm2]
Izz = 0.003+0.3914;     %[Kgm2]
g   = 9.82;
CG  = 0.32;             %[m]


% Rigid bodies properties
DYDATA.Parameters_6DOF = [M,     g,  CG;
                          Ixx, Iyy, Izz;
                          0,     0,   0];


return