import numpy as np


class NumQuaternion:
    def _vector_product(self, v1, v2):
        """vectors format: numpy.array([[0.0], [0.0], [0.0]])"""
        result = np.array([[0.0], [0.0], [0.0]])
        result[0, 0] = v1[1, 0] * v2[2, 0] - v1[2, 0] * v2[1, 0]
        result[1, 0] = v1[2, 0] * v2[0, 0] - v1[0, 0] * v2[2, 0]
        result[2, 0] = v1[0, 0] * v2[1, 0] - v1[1, 0] * v2[0, 0]
        return result

    def _mixed_product(self, v1, v2, v3):
        mart = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]])
        mart[0, 0], mart[0, 1], mart[0, 2] = v1[0, 0], v1[1, 0], v1[2, 0]
        mart[1, 0], mart[1, 1], mart[1, 2] = v2[0, 0], v2[1, 0], v2[2, 0]
        mart[2, 0], mart[2, 1], mart[2, 2] = v3[0, 0], v3[1, 0], v3[2, 0]
        return np.linalg.det(mart)

    def _abs_vector(self, vector):
        """vector format: numpy.array([[0.0], [0.0], [0.0]])"""
        return ((vector[0, 0]) ** 2 + (vector[1, 0]) ** 2 + (vector[2, 0]) ** 2) ** 0.5

    def _prod_quaternions(self, q1, q2):
        Q = NumQuaternion()
        Q.scalar = q1.scalar * q2.scalar - np.vdot(
            np.array(q1.vector), np.array(q2.vector)
        )
        Q.vector = (
            q1.scalar * q2.vector
            + q2.scalar * q1.vector
            + self._vector_product(q1.vector, q2.vector)
        )
        return Q

    def _conjugate(self):
        Q = NumQuaternion()
        Q.scalar = self.scalar
        Q.vector = self.vector * (-1.0)
        return Q

    def _norma(self):
        return self.scalar**2 + np.vdot(self.vector, self.vector)

    def _inv(self):
        Q = NumQuaternion()
        _norma = self._norma()
        sopr = self._conjugate()
        if _norma != 0:
            Q.scalar = np.asarray(sopr.scalar).item() / np.asarray(_norma).item()
            Q.vector = sopr.vector / _norma
        else:
            raise ValueError("error: zero _norma NumQuaternion")
        return Q

    def rotate_vector(self, vector):
        """np.array([0, 0, 0])"""
        Q = NumQuaternion()
        Q.scalar = 0.0
        Q.vector = np.array([[vector[0]], [vector[1]], [vector[2]]])
        sopr = self._conjugate()
        result = self._prod_quaternions(self, self._prod_quaternions(Q, sopr)).vector
        return np.array([result[0, 0], result[1, 0], result[2, 0]])

    def rotate_basis(self, vector):
        """np.array([0, 0, 0])"""
        Q = NumQuaternion()
        Q.scalar = 0.0
        Q.vector = np.array([[vector[0]], [vector[1]], [vector[2]]])
        sopr = self._conjugate()
        result = self._prod_quaternions(sopr, self._prod_quaternions(Q, self)).vector
        return np.array([result[0, 0], result[1, 0], result[2, 0]])

    def get_matrix(self):
        M = np.array([[0.0, 0, 0], [0, 0, 0], [0, 0, 0]])
        L0 = self.scalar
        L1 = self.vector[0, 0]
        L2 = self.vector[1, 0]
        L3 = self.vector[2, 0]

        # First row
        M[0, 0] = 1 - 2 * L2 * L2 - 2 * L3 * L3
        M[0, 1] = 2 * L1 * L2 - 2 * L3 * L0
        M[0, 2] = 2 * L1 * L3 + 2 * L2 * L0

        # Second row
        M[1, 0] = 2 * L1 * L2 + 2 * L3 * L0
        M[1, 1] = 1 - 2 * L1 * L1 - 2 * L3 * L3
        M[1, 2] = 2 * L2 * L3 - 2 * L1 * L0

        # Third row
        M[2, 0] = 2 * L1 * L3 - 2 * L2 * L0
        M[2, 1] = 2 * L2 * L3 + 2 * L1 * L0
        M[2, 2] = 1 - 2 * L1 * L1 - 2 * L2 * L2
        return M

    def __init__(self, angle=np.pi, axis=[0.0, 0.0, 0.0]):
        """angle - angle of the final rotation, e - direction of the axis of rotation
        axis format: np.array([0, 0, 0])"""
        self.vector = np.array([[0.0], [0.0], [0.0]])
        e = np.array([[axis[0]], [axis[1]], [axis[2]]])
        if self._abs_vector(e) != 0:
            ee = e / self._abs_vector(e)
        else:
            ee = e
        self.scalar = np.cos(angle / 2)
        self.vector = ee * np.sin(angle / 2)
