function [eul, eulAlt] = quat2eul( q, varargin )
%QUAT2EUL Convert quaternion to Euler angles
%   EUL = QUAT2EUL(QOBJ) converts a quaternion object, QOBJ, into the
%   corresponding Euler angles, EUL. Each quaternion represents
%   a 3D rotation. QOBJ is an N-element vector of quaternion objects.
%   The output, EUL, is an N-by-3 array of Euler rotation angles with each
%   row representing one Euler angle set. Rotation angles are in radians.
%
%   EUL = QUAT2EUL(Q) converts a unit quaternion rotation into the
%   corresponding Euler angles. The input, Q, is an N-by-4 matrix
%   containing N quaternions. Each quaternion represents a 3D rotation and
%   is of the form q = [w x y z], with w as the scalar number. Each element
%   of Q must be a real number.
%
%   EUL = QUAT2EUL(___, SEQ) converts unit quaternion into Euler angles.
%   The Euler angles are specified by the body-fixed (intrinsic) axis
%   rotation sequence, SEQ.
%
%   [EUL, EULALT] = QUAT2EUL(___) also returns a second output, EULALT,
%   which is a different euler representation of the same 3D rotation.
%
%   The default rotation sequence is 'ZYX', where the order of rotation
%   angles is Z Axis Rotation, Y Axis Rotation, and X Axis Rotation.
%
%   The following rotation sequences, SEQ, are supported: 'ZYX', 'ZYZ', and
%   'XYZ'.
%
%   Example:
%      % Calculates Euler angles for a quaternion
%      % By default, the ZYX axis order will be used.
%      q = [sqrt(2)/2 0 sqrt(2)/2 0];
%      eul = quat2eul(q)
%
%      % Calculate the Euler angles for a ZYZ rotation
%      qobj = quaternion([0.7071 0.7071 0 0]);
%      eulZYZ = quat2eul(qobj, 'ZYZ')
%
%   See also eul2quat, quaternion

%   Copyright 2014-2023 The MathWorks, Inc.

%#codegen

% Validate the quaternions
    q = robotics.internal.validation.validateQuaternion(q, 'quat2eul', 'q');

    seq = robotics.internal.validation.validateLegacyEulerSequence(varargin{:});

    % Normalize the quaternions
    q = robotics.internal.normalizeRows(q);

    qw = q(:,1);
    qx = q(:,2);
    qy = q(:,3);
    qz = q(:,4);

    % Pre-allocate output
    eul = zeros(size(q,1), 3, 'like', q);

    % The parsed sequence will be in all upper-case letters and validated
    switch seq
      case 'ZYX'
        % Cap all inputs to asin to 1, since values >1 produce complex
        % results
        % Since the quaternion is of unit length, this should never happen,
        % but some code generation configuration seem to hit this edge case
        % under some circumstances.
        aSinInput = -2*(qx.*qz-qw.*qy);
        mask1 = aSinInput >= 1 - 10*eps(class(qw));
        mask2 = aSinInput <= -1 + 10*eps(class(qw));
        aSinInput(mask1) = 1;
        aSinInput(mask2) = -1 ;
        mask = mask1 | mask2;

        eul = [ atan2( 2*(qx.*qy+qw.*qz), qw.^2 + qx.^2 - qy.^2 - qz.^2 ), ...
                asin( aSinInput ), ...
                atan2( 2*(qy.*qz+qw.*qx), qw.^2 - qx.^2 - qy.^2 + qz.^2 )];
        eul(mask, 1) = -sign(aSinInput(mask,1)).* 2 .* atan2(qx(mask,1), qw(mask,1));
        eul(mask, 3) = 0;

      case 'ZYZ'
        % Need to convert to intermediate rotation matrix here to avoid
        % singularities
        R = quat2rotm(q);
        eul = rotm2eul(R, 'ZYZ');

      case 'XYZ'
        % Prevent singularities as done in ZYX case
        % Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
        % operations

        % sin(y) = R13 = 2 * (qx*qz + qy*qw)
        % tan(x) = sin(x) / cos(x) = -R23 / R33
        %        = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
        %        = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
        % tan(z) = sin(z) / cos(z) = -R12 / R11
        %        = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
        %        = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)

        aSinInput = 2*(qx.*qz + qy.*qw);
        mask1 = aSinInput >= 1 - 10*eps(class(qw));
        mask2 = aSinInput <= -1 + 10*eps(class(qw));
        aSinInput(mask1) = 1;
        aSinInput(mask2) = -1;
        mask = mask1 | mask2;

        eul = [ atan2( -2*(qy.*qz - qx.*qw), qw.^2 - qx.^2 - qy.^2 + qz.^2 ), ...
                asin( aSinInput ), ...
                atan2( -2*(qx.*qy - qz.*qw), qw.^2 + qx.^2 - qy.^2 - qz.^2 )];
        eul(mask, 1) = 0;
        eul(mask, 3) = sign(aSinInput(mask,1)) .* 2.*atan2(qx(mask,1), qw(mask,1));
    end

    % Check for complex numbers
    if ~isreal(eul)
        eul = real(eul);
    end

    if nargout > 1
        eulAlt = robotics.core.internal.generateAlternateEulerAngles(eul, seq);
    end

end
