function [ RotationMatrix ] = Rotation( alpha, beta , gamma, MatrixType )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

%nargin=4 no problem

if nargin == 3  
	MatrixType = 'RPY';
end
if nargin < 3 
    %I need alpha, beta , gamma 
    RotationMatrix = zeros(10,10)  ;
    return; 
end


%%Convert degrees to radian and store them the same variables..
alpha = degtorad(alpha);
beta = degtorad(beta);
gamma = degtorad(gamma);

CA = cos(alpha); SA = sin(alpha);
CB = cos(beta);  SB = sin(beta);
CG = cos(gamma); SG = sin(gamma);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% L.-W. Tsai, “Robot analysis: the mechanics of serial and parallel manipulators”, Wiley-IEEE, 1999.
%%%% Page 38  (1.43)
%%%% Roll-Pitch-Yaw Angles
if ( strcmp( MatrixType , 'RPY' ) ==  1   )

RotationMatrix = ...
    [ CG*CB     -SG*CA+CG*SB*SA      SA*SG+CG*SB*CA
      SG*CB      CG*CA+SG*SB*SA     -CG*SA+SG*SB*CA
       -SB            CB*SA              CB*CA
    ];
    
%     
%  RotationMatrix = ...
%     [ CA*CB     -SA*CG+CA*SB*SG     SA*SG+CA*SB*CG
%       SA*CB      CA*CG+SA*SB*SG     -CA*SG+SA*SB*CG
%        -SB            CB*SG              CB*CG
%     ];

%     RotationMatrix2 =[ cos(alpha)*cos(beta) , cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma)...
%      , cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma) ;
%        sin(alpha)*cos(beta) , sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma) ,  sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma) ;
%        -sin(beta)           ,        cos(beta)*sin(gamma)                         ,  cos(beta)*cos(gamma)     ;
%        ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   

%% Roll-Pitch-Yaw Angles
else if ( strcmp( MatrixType , 'RPY-ZYX' ) ==  1   )

 RotationMatrix=...
    [       CB*CG           -CB*SG             SB; 
      CA*SG+CG*SA*SB    CA*CG-SA*SB*SG      -CB*SA; 
      SA*SG-CA*CG*SB    CG*SA+CA*SB*SG       CA*CB
    ];

% 
% else if ( strcmp( MatrixType , 'EULER' ) ==  1   )
% 
%  RotationMatrix=...
%     [       CB*CG           -CB*SG             SB; 
%       CA*SG+CG*SA*SB    CA*CG-SA*SB*SG      -CB*SA; 
%       SA*SG-CA*CG*SB    CG*SA+CA*SB*SG       CA*CB
%     ];
% 

   
else
     RotationMatrix = zeros(10,10) ;
end

end

