# ==============================================================================
# Copyright (C) 2023 Haresh Rengaraj Rajamohan, Tianyu Wang, Kevin Leung, 
# Gregory Chang, Kyunghyun Cho, Richard Kijowski & Cem M. Deniz 
#
# This file is part of OAI-MRI-TKR
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Affero General Public License as published
# by the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.

# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Affero General Public License for more details.

# You should have received a copy of the GNU Affero General Public License
# along with this program.  If not, see <https://www.gnu.org/licenses/>.
# ==============================================================================
import random
import numpy as np
import math

def angle_axis_to_rotation_matrix(angle,axis):

    '''
    :param angle: angle of rotation
    :param axis: Axis 0f rotation
    :return: Rotation Matrix
    '''

    A = np.outer(axis,axis)
    B = np.zeros((3,3),dtype=float)
    B[0,1] = -1*axis[-1]
    B[0,2] = axis[-2]
    B[1,2] = axis[0]
    B = B - B.transpose()
    R = math.cos(angle)+np.eye(3)+ math.sin(angle)*B + (1-math.cos(angle))*A
    return R


def generating_random_rotation_matrix():
    '''
    :return: random rotation matrix, axis of rotation, angle of rotation
    random rotation matrix is selected in such a way that the axis of rotation is uinformally distributed on a unit
    sphere and the angle is uniformally distributed.
    '''

    guassian_vector = [random.gauss(mu=0, sigma=1), random.gauss(mu=0, sigma=1), random.gauss(mu=0, sigma=1)]
    axis_of_rotation = list(np.array(guassian_vector) / np.linalg.norm(x=np.array(guassian_vector)))
    angle_of_rotation = 2 * math.pi * random.uniform(0, 1)
    rotation_matrix = angle_axis_to_rotation_matrix(angle=angle_of_rotation, axis=axis_of_rotation)
    return rotation_matrix, axis_of_rotation, angle_of_rotation