import math


def Rmat_to_thetas(R):
    """
    Extract Tait-Bryan angles from rotation matrix
    :param R: 
    :return: len-3 list of theta angles x, y, z in radian
    """
    singular_y = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    th_y = math.atan2(-R[2, 0], singular_y)  # similar to asin(-R[2, 0])
    singular = singular_y < 1e-6

    if not singular:
        # most of the time we fall in the non-singulare case
        th_x = math.atan2(R[2, 1], R[2, 2])
        th_z = math.atan2(R[1, 0], R[0, 0])
    else:
        # When th_y tends to pi/2 or -pi/2, then x and z rotation are mixed together leading to an infinity of solution.
        # So we set th_z to 0 to have a single solution
        th_x = math.atan2(-R[1, 2], R[1, 1])
        th_z = 0

    thetas = [th_x, th_y, th_z]
    return thetas