function Output = versatile_function(task,varargin)
%{
Function for performing different geometrically oriented tasks.
--------------------------------------------------------------------------
Syntax :
Output = versatile_function(task,varargin)
--------------------------------------------------------------------------
Function Description :
This function is dedicated to perform different geometrically oriented
tasks. It receives the task description from the 'task' input based on
which it choses which routine has to be executed. Here are the lists of the
a priori defined tasks:

            1- 'define Rot_H2T' : it defines the rotation matrix of humerus
            to thorax based on the current generalized coordinates inputted
            by the varargin.

            2- 'define partial velocities matrix' : it defines the partial
            velocities matrix associated with the rotational generalizde
            coordinates.

            3- 'define DRot_H2T' : it defines the time differentiation of
            the rotation matrix of humerus to thorax based on the inputted
            values for the generalized coordinates and their associated
            rates of change from the varargin.

            4- 'define Rot_G2T' : it defines the rotation matrix of the
            glenoid to thorax based on the inputted values for the
            generalized coordinates and the scapulohumeral rhythm.
--------------------------------------------------------------------------
%}
% 1- 'define Rot_H2T' : it defines the rotation matrix of humerus to thorax
% based on the current generalized coordinates inputted by the varargin.
            
if isequal(task,'define Rot_H2T')
    
    q = varargin{1,1}; % input the generalized coordinates
    
    R_psi = [cos(q(1)) 0 sin(q(1)); 
             0       1     0; 
             -sin(q(1)) 0  cos(q(1))];

    R_theta = [1     0         0;
               0  cos(q(2))  -sin(q(2));
               0  sin(q(2))   cos(q(2))];
  
    R_phi = [cos(q(3)) 0 sin(q(3)); 
             0      1     0; 
             -sin(q(3)) 0  cos(q(3))];

    R_H2T = R_phi*R_theta*R_psi;
    
    Output = [R_H2T];
    
% ------------------------------------------------------------------------    
% 2- 'define partial velocities matrix' : it defines the partial velocities
% matrix associated with the rotational generalizde coordinates.
    
elseif isequal(task,'define partial velocities matrix')
    
    q = varargin{1,1}; % input the generalized coordinates
    
    e = [sin(q(2))*sin(q(3)) cos(q(2)) cos(q(3))*sin(q(2));
         cos(q(3))           0         -sin(q(3))         ;
         0                   1         0                 ]; % partial velocities matrix
    
    Output = e;

% -------------------------------------------------------------------------    
% 3- 'define DRot_H2T' : it defines the time differentiation of the rotation
% matrix of humerus to thorax based on the inputted values for the generalized
% coordinates and their associated rates of change from the varargin.
            
elseif isequal(task,'define DRot_H2T')
    
    q = varargin{1,1}; % input the generalized coordinates
    
    R_psi = [cos(q(1)) 0 sin(q(1)); 
             0       1     0; 
             -sin(q(1)) 0  cos(q(1))];

    R_theta = [1     0         0;
               0  cos(q(2))  -sin(q(2));
               0  sin(q(2))   cos(q(2))];
  
    R_phi = [cos(q(3)) 0 sin(q(3)); 
             0      1     0; 
             -sin(q(3)) 0  cos(q(3))];

    DR_psi = [-q(7)*sin(q(1)) 0 q(7)*cos(q(1)); 
              0               0              0; 
             -q(7)*cos(q(1))  0  -q(7)*sin(q(1))];

    DR_theta = [0  0                              0;
                0  -q(8)*sin(q(2))  -q(8)*cos(q(2));
                0  q(8)*cos(q(2))   -q(8)*sin(q(2))];
  
    DR_phi = [-q(9)*sin(q(3)) 0 q(9)*cos(q(3)); 
              0               0              0; 
             -q(9)*cos(q(3))  0  -q(9)*sin(q(3))];         
         
    DR_H2T = DR_phi*R_theta*R_psi + R_phi*DR_theta*R_psi + R_phi*R_theta*DR_psi;
    
    Output = [DR_H2T];
    
% -------------------------------------------------------------------------    
% 4- 'define Rot_G2T' : it defines the rotation matrix of the glenoid to
% thorax based on the inputted values for the generalized coordinates and
% the scapulohumeral rhythm.  

elseif isequal(task,'define Rot_G2T')
    
    q = varargin{1,1}; % input the generalized coordinates
        
    % define the glenoid motion using the scapulohumeral rhythm:
    if ge(q(2),-pi/6) % or IDDATA.Coordinate_Evolution(2,j)ImposedMotion(4,j)
                                           % if full-state convergence's achieved
         qg = 0;
    else
         qg = (1/2)*(q(2)+pi/6);
    end
    
    R_G2T = [1        0         0;
             0  cos(qg)  -sin(qg);
             0  sin(qg)   cos(qg)]; % glenoid to thorax rotation matrix
         
    Output = [R_G2T];     
% ------------------------------------------------------------------------  
% error message

else
    error('using undefined task for function "versatile_function"');
 
end
    return;