function IDDATA = inverse_dynamic_simulation_new1(DYDATA)

%{
Function for solving the inverse-dynamics problem.
--------------------------------------------------------------------------
Syntax :
IDDATA = inverse_dynamic_simulation_new1(DYDATA)
--------------------------------------------------------------------------
Function Description :
This function solves the inverse-dynamics problem. We first defines the
motion to be imposed on the system (psi, theta, and phi). We use the system
equations of motion to end up with an algebraic equation like Ax=b. In the
meanwile we have defined the muscle paths. As the system is overactuated
with more actuators than the degrees of freedom, we use two succesive
optimizations to address the indeterminancy of the equation Ax=b.
At the end, we have the muscle forces, muscle lines of actions, their lever
arms, and we also define the joint reaction force. The joint reaction force
is associated with the ideal representation of the glenohumeral joint (with
three degrees of freedom and no translations).
--------------------------------------------------------------------------
%}

% ------------------------------------------------------------------------
% define the kinematics
% ------------------------------------------------------------------------

IDDATA.StepSize = 0.5;           % stepsize of the whole simulation
t = [0.01:IDDATA.StepSize:7.2];  % time sedgment
IDDATA.t = t;                    % keeping the time somewhere for possible
                                 % use elsewhere

for j=1:length(t)

    disp(['solving inverse dynamic simulation, Time Stamp : ', num2str(j), '/', num2str(size(IDDATA.t,2))]);

% defining the kinematic (q, Dq, DDq) to impose to the model (q=psi, theta, phi)
% we are using the psi/theta/phi fixed angles representation which is
% equivalent with the phi/theta/psi Euler angle sequence. In other words,
% it's YXY. The rotations are considered to be around the inertial 
% coordinate {T} to rotate the body-fixed coordinate {H}.

TC = 20;                         % how fast the arm moves

psi   = ((pi/4)*(cos(2*pi*t(j)/TC)-1));
Dpsi  = (-(2/4)*(pi^2/TC)*sin(2*pi*t(j)/TC));
DDpsi = (-(4/4)*(pi^3/(TC^2))*cos(2*pi*t(j)/TC));

theta_abs    = (pi/2)*(cos(2*pi*t(j)/TC)-1);%
Dtheta_abs   = -(pi^2/TC)*sin(2*pi*t(j)/TC);%
DDtheta_abs  = -2*(pi^3/(TC^2))*cos(2*pi*t(j)/TC);%

phi   = ((pi/8)*(cos(2*pi*t(j)/TC)-1));
Dphi  = (-0.25*(pi^2/TC)*sin(2*pi*t(j)/TC));
DDphi = (-0.5*(pi^3/(TC^2))*cos(2*pi*t(j)/TC));

IDDATA.ImposedMotion(:,j) = [psi Dpsi DDpsi theta_abs Dtheta_abs...
                             DDtheta_abs phi Dphi DDphi]';
    
    %scapula motion is restricted to humerus by scapulohumeral rhythm
    if ge(theta_abs,-pi/15)
        theta_s = 0;
    else
        theta_s = (1/1.8)*(theta_abs+pi/15);
    end
    
    % rotation matrices. S,H, and T stand for scapula, humerus, and
    % thorax, respectively. Thorax is the inertial frame.

    Rot_S2T = [1 0 0;
               0 cos(theta_s) -sin(theta_s);
               0 sin(theta_s) cos(theta_s)]; % scapula to thorax
    Rot_T2S = Rot_S2T'; 
    
    R_phi = [cos(phi) 0 sin(phi);
             0        1         0;
             -sin(phi) 0 cos(phi)]; 
    R_tht = [1        0         0;
             0 cos(theta_abs) -sin(theta_abs);
             0 sin(theta_abs) cos(theta_abs)];    
    R_psi = [cos(psi) 0 sin(psi);
             0        1         0;
             -sin(psi) 0 cos(psi)]; 
     

    Rot_H2T = R_phi*R_tht*R_psi; % humerus to thorax  
    Rot_T2H = Rot_H2T';    
    
% ------------------------------------------------------------------------
% wrapping algorithm, define the muscle path
% ------------------------------------------------------------------------

    % define the muscles path: force direction and the lever arm (wrapping algorithm)
    [Rho_inT,Dir_inT]= muscle_wrapping(Rot_S2T,Rot_H2T);
    
    IDDATA.LeverArm{j} = Rho_inT(:,:);        % save the leverarms if one to plot
    IDDATA.ForceDirection{j} = Dir_inT(:,:);  % save the focre directions if one
                                              % to plot

% ------------------------------------------------------------------------
% formulate the inverse-dynamics problem
% ------------------------------------------------------------------------
    % the constants
    M = DYDATA.Parameters(1,1);   Ixx = DYDATA.Parameters(2,1);
    g = DYDATA.Parameters(1,2);   Iyy = DYDATA.Parameters(2,2);
    CG = DYDATA.Parameters(1,3);  Izz = DYDATA.Parameters(2,3);

    % calling the imposed motion
    q1 = psi;          dq1 = Dpsi;
    q2 = theta_abs;    dq2 = Dtheta_abs;
    q3 = phi;          dq3 = Dphi;
    
    % Re-write the equations of motion introducing the values above
    EquationsofMotion
    
    SysDyn = MDYN*[DDpsi DDtheta_abs DDphi]' - RHS; % stands for b in the Ax=b
    
    IDDATA.GeneralizedForces(:,j) = SysDyn; % save the generalized forces
    
    % partial velocities vector. There are different ways of defining this.
    % One intuitive way is to define the non-orthogonal directions around
    % which the Euler angle rotations have been perfomred. They can then be
    % defined in the body-fixed frame and the inertial frame. Here we
    % define everything in the inertial frame {T}. The other way of
    % defining this is to use the straighforward formula: e_qi =
    % dW/d(dqi) (W angular velocities).
    % One can ask why we are defining this? Because we need to
    % know the generalized forces for the right-hand side of the Lagrange
    % equations. It can be shown that the generalized forces can be derived
    % as sigma M*e_qi for each of the qi when we only have rotations in the
    % system.
    
    PartialVel = [sin(phi)*sin(theta_abs) cos(theta_abs) cos(phi)*sin(theta_abs);
                  cos(phi) 0 -sin(phi);
                  0 1 0];
              
    % generalized moment arm matrix. W*F = generlized forces
    W = PartialVel*[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))];

% ------------------------------------------------------------------------
% solve the load-sharing problem. SysDyn = W*F <-----> b = Ax
% ------------------------------------------------------------------------
   
    [F_p,F_s] = load_sharing(SysDyn,W); 
    
    % final answers including both special and particular answers
    IDDATA.MuscleForces(:,j) = F_p + F_s;
    
% ------------------------------------------------------------------------
% define the reaction force
% ------------------------------------------------------------------------    
    % the second derivatives of the imposed motion
    ddq1 = DDpsi;  ddq2 = DDtheta_abs;  ddq3 = DDphi;       
    
    % the linear acceleration of the humerus' center of gravity is defined
    % below. We use the Newton law of motion, to define the reaction force.
    % sigma F = m*a. It's like we already used the Euler's equitions for
    % the rotational part of the system movement, and now we use Newton for
    % the tranlational part. However, there is no tranlsations involved in
    % the equations given that the A_CG does not have any tranlational
    % part. A_CG = A_O + A_rel, A_O = 0; But, if we had tranlations then
    % the A_CG would be a function of tranlational generalized coordinates.
    % on one side it would be the unknown reaction force and the known
    % muscle forces and the gravity force and on the other side the second
    % differentiations of the tranlational generalized coordinates. So we
    % cannot solve it, otherwise we define the reaction forces in a way.
    LinearAccofCenterofMass
    
    IDDATA.JointReaction(:,j) = M*A_CG - [0 -M*g 0]' - (Dir_inT*IDDATA.MuscleForces(:,j));
    IDDATA.JointReactionMagnitude(j)=norm(IDDATA.JointReaction(:,j)); % joint reaction force magnitude
    

end

% ------------------------------------------------------------------------
% plot the results of the inverse-dynamics problem
% ------------------------------------------------------------------------
figure    
plot(IDDATA.t(1,:),IDDATA.MuscleForces(:,:),...
    IDDATA.t(1,:),IDDATA.JointReactionMagnitude(:))
return