function [euler_angles,R2]= Euler_from_R(R)

% This function return the rotation angles along x,y and z direction from a 
% Rotation Matrix

%Inputs:
    % R= 3x3 Rotation Matrix
%Outputs:
    % rx= Rotation along x direction in radians
    % ry= Rotation along y direction in radians
    % rz= Rotation along z direction in radians
    
%     R =
%  
% [                           cos(ry)*cos(rz),                          -cos(ry)*sin(rz),          sin(ry)]
% [ cos(rx)*sin(rz) + cos(rz)*sin(rx)*sin(ry), cos(rx)*cos(rz) - sin(rx)*sin(ry)*sin(rz), -cos(ry)*sin(rx)]
% [ sin(rx)*sin(rz) - cos(rx)*cos(rz)*sin(ry), cos(rz)*sin(rx) + cos(rx)*sin(ry)*sin(rz),  cos(rx)*cos(ry)]

% Expanded by Mimmo Gatti from the original written by Sandeep Sasidharan
% (http://sandeepsasidharan.webs.com), by adding a check for all the
% possible sign permutations in order to obtain correct signs for the Euler
% angles

ry = asin(R(1,3));
rz = acos(R(1,1)/cos(ry));
rx = acos(R(3,3)/cos(ry));

r2d = 180/pi;
rx = r2d*rx;
ry = r2d*ry;
rz = r2d*rz;    

% Here we save the original angles before all the sign permutations
o_rx = abs(rx);
o_ry = abs(ry);
o_rz = abs(rz);

% Here we create an array with all the possible sign permutations
sign_perms = perms([-1 -1 -1 1 1 1]);
sign_perms = sign_perms(:,1:3);
sign_perms = unique(sign_perms,'rows');

nperms = size(sign_perms,1);
sum_dif = zeros(nperms,1);
t_rx = zeros(nperms,1);
t_ry = zeros(nperms,1);
t_rz = zeros(nperms,1);
R2_stack = zeros(3,3,nperms);

% Here we calculate a rotation matrix with each sign permutation and we
% compare it with the original rotation matrix
for i = 1:nperms
    rx = o_rx*sign_perms(i,1);
    ry = o_ry*sign_perms(i,2);
    rz = o_rz*sign_perms(i,3);
    
R2 = R_from_Euler(rx,ry,rz);
sum_dif(i) = sum(abs(R(:)- R2(:)));
t_rx(i) = rx;
t_ry(i) = ry;
t_rz(i) = rz;
R2_stack(:,:,i) = R2;
end

% Here we select the rotations with the sign combinations that give the
% rotation matrix numerically closer to the original
[~,min_ind] = min(sum_dif);
best_rx = t_rx(min_ind);
best_ry = t_ry(min_ind);
best_rz = t_rz(min_ind);
R2 = R2_stack(:,:,min_ind);

% Finally we export the best Euler angles
euler_angles = [best_rx best_ry best_rz];    

