import numpy as np
import math


def _normalize(vec: np.ndarray):
    return vec / np.linalg.norm(vec)


def _look_at(eye: np.ndarray, center: np.ndarray, up: np.ndarray):
    eye = np.asanyarray(eye)
    center = np.asanyarray(center)
    up = np.asanyarray(up)

    z = _normalize(eye - center)
    x = np.cross(up, z)
    y = np.cross(z, x)

    x = _normalize(x)
    y = _normalize(y)

    rotation_matrix = np.array(
        [
            [x[0], y[0], z[0]],
            [x[1], y[1], z[1]],
            [x[2], y[2], z[2]],
        ],
        dtype=np.float32,
    )
    translation_vector = np.array([-x @ eye, -y @ eye, -z @ eye])

    return rotation_matrix, translation_vector


def get_extrinsics():
    camera_theta = math.pi / 3
    camera_phi = -math.pi / 4
    camera_r = 1
    eye = [
        camera_r * math.sin(camera_theta) * math.cos(camera_phi),
        camera_r * math.cos(camera_theta),
        camera_r * math.sin(camera_theta) * math.sin(camera_phi),
    ]
    rotation_matrix, translation_vector = _look_at(eye=eye,
                                                   center=[0, 0, 0],
                                                   up=[0, 1, 0])
    translation_vector = translation_vector[None, :]
    rotation_matrix = np.linalg.inv(rotation_matrix)

    return rotation_matrix, translation_vector
