function [DYDATA] = initialize_dynamic_data()
%{
Function for deriving the equations of motion.
--------------------------------------------------------------------------
Syntax :
[DYDATA] = initialize_dynamic_data()
--------------------------------------------------------------------------
Function Description :

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

% Let's define the generalized coordinates
syms q1 q2 q3 real;        %generalized coordinates
syms dq1 dq2 dq3 real;     %generalized velocities
syms ddq1 ddq2 ddq3 real;  %generalized accelerations


% and the generalized coordinates/velocities/accelerations vectors
q   = [q1; q2; q3];
dq  = [dq1; dq2; dq3];
ddq = [ddq1; ddq2; ddq3];

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

% Inertia tensor
I = [Ixx 0 0; 0 Iyy 0; 0 0 Izz];

% Defining the rotation matrix for a typical 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)];

Ryxy = R_phi*R_theta*R_psi; % local to global

% we define the spatial configuration of the body by the position of its
% center of mass in the inertial frame
OG = Ryxy*[0; -CG; 0];

% here is the angular velocity of the body in the body fixed (humerus) frame:

Wrotation = dq3*R_psi'*R_theta'*[0 1 0]' + dq2*R_psi'*[1 0 0]' + dq1*[0 1 0]';

% angular velocity of the body in the inertial frame (thorax):
Wrotation = simplify(expand(Ryxy*Wrotation));

% linear acceleration 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*Wrotation'*I*Wrotation;

% Potential energy
V = M*g*dot([0 1 0]',OG);

% 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.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.m','w');
for i=1:N4
    fprintf(fileID,'A_CG(%d,1) = %s;\n', i, char(A_CG(i)));
end
fclose(fileID);


% Generalized forces

Dir_inT = sym('Dir_inT',[3 6], 'real');     % direction
Rho_inT = sym('Rho_inT',[3 6], 'real');     % OP
F_final = sym('F_final',[1 6], 'real');     % magnitude

% partial angular velocities in the Thorax frame (dWrotation/ddq)
e = [sin(q2)*sin(q3) cos(q2) cos(q3)*sin(q2);
     cos(q3) 0 -sin(q3);
     0 1 0];

% genealized forces
Q = e*[cross(Rho_inT(:,1),Dir_inT(:,1)), cross(Rho_inT(:,2),Dir_inT(:,2)),...
    cross(Rho_inT(:,3),Dir_inT(:,3)), cross(Rho_inT(:,4),Dir_inT(:,4)),...
    cross(Rho_inT(:,5),Dir_inT(:,5)), cross(Rho_inT(:,6),Dir_inT(:,6))]*...
    F_final';

for i=1:3
    str=char(Q(i));
    str = strrep(str, 'al1', 'al(1)');
    str = strrep(str, 'al2', 'al(2)');
    str = strrep(str, 'al3', 'al(3)');
    str = strrep(str, 'al4', 'al(4)');
    str = strrep(str, 'al5', 'al(5)');
    str = strrep(str, 'al6', 'al(6)');
    
    str = strrep(str, '1_1', '(1,1)');
    str = strrep(str, '1_2', '(1,2)');
    str = strrep(str, '1_3', '(1,3)');
    str = strrep(str, '1_4', '(1,4)');
    str = strrep(str, '1_5', '(1,5)');
    str = strrep(str, '1_6', '(1,6)');
    
    str = strrep(str, '2_1', '(2,1)');
    str = strrep(str, '2_2', '(2,2)');
    str = strrep(str, '2_3', '(2,3)');
    str = strrep(str, '2_4', '(2,4)');
    str = strrep(str, '2_5', '(2,5)');
    str = strrep(str, '2_6', '(2,6)');
    
    str = strrep(str, '3_1', '(3,1)');
    str = strrep(str, '3_2', '(3,2)');
    str = strrep(str, '3_3', '(3,3)');
    str = strrep(str, '3_4', '(3,4)');
    str = strrep(str, '3_5', '(3,5)');
    str = strrep(str, '3_6', '(3,6)');
    Q(i)=cellstr(str);
end

fileID = fopen('GeneralizedForces.m','w');
for i=1:3
    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 = [M,     g,  CG;
                     Ixx, Iyy, Izz;
                     0,     0,   0];


return