"""
Rotations.
"""

import numpy as np
cimport numpy as np
cimport cython

import random

from libc.math cimport cos, sin, acos, log, exp, sqrt
from libc.math cimport M_PI

cdef:
    double TWOPI = 2 * M_PI


from cython_gsl cimport *
gsl_rng_env_setup()
cdef:
    const gsl_rng_type * T
    gsl_rng * r
T = gsl_rng_default
r = gsl_rng_alloc (T)

from cryoem.model.svd import svd3x3
from numpy.linalg import svd
from cryoem.model.math cimport vonmisesvariate
from numpy.linalg.linalg import norm
from cryoem.model.linalg cimport subtract3x3, cross, norm3d
from cryoem.model.linalg cimport add3x3, scale3x3, multiply3x3
from cryoem.model.linalg cimport norm_squared3d, to_skew_symmetric
from cryoem.model.linalg cimport copy3x3, hadamard3x3

cdef class Rotation:
    """Rotation of n-dimensional vectors where n>1."""

    def __cinit__(self, matrix):
        """Initialise rotation as a matrix."""

        self._matrix = matrix

    @property
    def matrix(self):
        return self._matrix

    @matrix.setter
    def matrix(self, value):
        self._matrix = value

    def set_matrix(self, value):
        self._matrix = value

    @property
    def dimension(self):
        return self._matrix.shape[0]

    def __call__(self, x):
        """Returns rotated vectors.

        If the rotation is a matrix R, then applying it to vector v
        gives Rv, i.e. it is applied from the left.

        Args:
            x: (N, D) array of data points to be rotated.
            y: (N, D) array where results should be stored.

        Returns:
            (N, D) array of rotated points.
        """

        return np.dot(np.atleast_2d(x), self.matrix.T)

    def inverse_rotate(self, x):
        """Returns rotated vectors by applying the inverse rotation."""

        return np.dot(np.atleast_2d(x), self.matrix)

    cpdef rotate(self, double[:, :] x, double[:, :] y):
        """Faster version for rotating vectors.

        If the rotation is a matrix R, then applying it to vector v
        gives Rv, i.e. it is applied from the left.

        Args:
            x: (N, D) array of data points to be rotated.
            y: (N, D) array where results should be stored.

        Note: when N=1 the vectors should still be 2 dimensional.
        """

        cdef:
            int i, j, k

        for i in range(y.shape[0]):
            for j in range(y.shape[1]):
                y[i, j] = 0.0
                for k in range(y.shape[1]):
                    y[i, j] += x[i, k] * self._matrix[j, k]

        # This is just implementing the following:
        ## y[:] = np.dot(x, self.matrix.T)


    @classmethod
    def random(cls):
        """Constructs a random rotation."""

        raise NotImplementedError

    def save(self, parent_group, name='rotation'):
        """Save to group in HDF5 database.

        Args:
            parent_group: Group within which to create the rotation group.
            name: Name of the new rotationgroup.
        """

        group = parent_group.create_group(name)
        rotation_dataset = group.create_dataset(
            'rotation',
            (<object>self._matrix).shape,
            np.double)
        rotation_dataset[...] = self.matrix

    @classmethod
    def load(cls, parent_group, name='rotation'):
        """Loads rotation from group in HDF5 database."""

        group = parent_group[name]
        matrix = group['rotation'][:]

        return cls(matrix)

    def copy(self):
        """Return deep copy."""

        matrix2 = np.array(self._matrix).copy()

        return self.__class__(matrix2)



    cpdef compose(self, other):
        """Return composition of this rotation with 'other'.

        First apply 'other' rotation, then this one.

        Previously: (25.3.2014)
        Return Rotation consisting of this one followed by 'other'.
        """

        return self.__class__(np.dot(np.array(self._matrix),
                                     np.array(other.matrix)))
        ## return self.__class__(np.dot(np.array(other.matrix),
        ##                              np.array(self._matrix)))


cdef class Rotation3D(Rotation):
    """Rotation of 3D vectors or 3D object.

    By convention this is the rotation of the object, not the frame.
    In the matrix representation, the two conventions are related
    through the transpose operation.
    """

    @classmethod
    def from_euler_angles(cls, euler_angles):
        """Construct a rotation from euler angles.

        From wikipedia article on 'Euler angles', using ZYZ convention.

        Args:
            euler_angles: (3, ) array with alpha, beta and gamma.
                The angles describe the movement of the coordinate frame.
                (Or is it the movement of the object?)

        Returns:
            Rotation3D object, describing the movement of the object.

        Note: returns the transpose of the matrix returned by
        commons.math.euler, and
        commons.geometry.rotations.euler_to_rotation.
        """

        c1, c2, c3 = np.cos(euler_angles)
        s1, s2, s3 = np.sin(euler_angles)

        matrix = np.array([[c1*c2*c3-s1*s3, -c3*s1-c1*c2*s3, c1*s2],
                           [c1*s3+c2*c3*s1, c1*c3-c2*s1*s3, s1*s2],
                           [-c3*s2, s2*s3, c2] ]).T

        return cls(matrix)

    @classmethod
    def from_quaternion(cls, quaternion):
        """Construct a rotation from a quaternion.

        From wikipedia article 'Quaternions and spatial rotation'.

        Args:
            quaternion: (4, ) array

        Returns:
            Rotation3D object, describing the movement of the object.
        """

        a, b, c, d = quaternion
        matrix = np.array([
            [a*a + b*b - c*c - d*d, 2*b*c - 2*a*d, 2*b*d + 2*a*c],
            [2*b*c + 2*a*d, a*a - b*b + c*c - d*d, 2*c*d - 2*a*b],
            [2*b*d - 2*a*c, 2*c*d + 2*a*b, a*a - b*b - c*c + d*d] ])

        return cls(matrix)

    @classmethod
    def random(cls):
        """Constructs a random rotation."""

        euler_angles = cls._random_euler_angles()

        return cls.from_euler_angles(euler_angles)

    @classmethod
    def identity(cls):

        return Rotation3D(np.eye(3))

    @classmethod
    def from_axis_angle(cls, axis, angle):
        """Return rotation of angle radians about axis."""

        t = angle / 2.0
        c = np.cos(t)
        s = np.sin(t)
        axis2 = axis / norm(axis)

        return cls.from_quaternion(np.append([c], s * axis2))

    @classmethod
    def between(cls, a, b):
        """Constructs rotation mapping vector a to vector b."""

        epsilon = 1e-6

        def normalise(x):
            return x / norm(x)

        a1 = normalise(a)
        b1 = normalise(b)

        # Check if a = -b.
        if norm(a1 + b1) < epsilon:
            if abs(a1[0]) < (1.0 - epsilon):
                other = np.array([1.0, 0.0, 0.0])
            else:
                other = np.array([0.0, 1.0, 0.0])
            return cls.from_axis_angle(np.cross(a1, other), np.pi)
            ## quaternion = np.append(normalised(np.cross(a1, other)), np.pi)
        else:
            half = normalise(a1 + b1)
            return cls.from_quaternion(np.append([np.dot(a1, half)],
                                                 np.cross(a1, half)))


    def to_euler_angles(self):
        """Return Euler angles for this rotation.

        Should be the inverse of 'from_euler_angles'.
        """

        r = np.array(self._matrix)

        alpha = np.arctan2(r[2,1],r[2,0]) % (2*np.pi)
        beta = (np.arctan2((r[2,0]+r[2,1]) /
                           (np.cos(alpha) + np.sin(alpha)),
                           r[2,2]) % (2*np.pi))
        gamma = np.arctan2(r[1,2],-r[0,2]) % (2*np.pi)

        return np.array([alpha, beta, gamma])

    def to_axis_angle(self):
        """Convert to axis-angle representation.

        From wikipedia 'Axis_angle', section 'Log map from SO(3) to so(3)'.
        Doesn't work when the rotation angle is very small.
        """

        r = np.array(self._matrix)
        angle = np.arccos((r[0,0]+r[1,1]+r[2,2] - 1)/2.0)
        d = 2.0 * np.sin(angle)
        axis = np.array([r[2,1]-r[1,2], r[0,2]-r[2,0], r[1,0]-r[0,1]]) / d

        return axis, angle

    def relative(self, other_rotation):
        """Return rotation A B^T, where A is this rotation, and B the other.

        Note that A^T B might be more appropriate.
        """
        r = np.array(self._matrix)
        r2 = np.array(other_rotation.matrix)

        ## return Rotation3D(np.dot(r.T, r2))
        return Rotation3D(np.dot(r, r2.T))


    @staticmethod
    def _random_euler_angles():
        """Generate random 3D rotation represented as Euler angles.

        From the wikipedia page on 'Euler angles', read the sentence
        starting with 'For example, to generate uniformly randomized
        orientations,' etc.
        This gives exactly this method.
        """
        alpha = 2*np.pi*np.random.random()

        cosbeta = 2*np.random.random() - 1
        beta = np.arccos(cosbeta)

        gamma = 2*np.pi*np.random.random()

        return np.array([alpha, beta, gamma])

    def invert(self):
        """Return rotation with transpose of matrix."""

        return Rotation3D(np.array(self._matrix).T)

    @classmethod
    def average(cls, rotations):
        """Compute average rotation.

        Args:
            rotations: List of Rotation3D objects.
        """

        matrices = np.dstack([np.array(r.matrix)
                              for r in rotations])
        mean = matrices.mean(axis=2)
        U, s, Vh = svd(mean)
        matrix = np.dot(U, Vh)

        return cls(matrix)

    def __str__(self):

        return str(np.array(self._matrix))

    @classmethod
    def generate_symmetry_group(cls, name):
        """Return list of Rotation3D objects.

        Supported names: 'Dn', for example 'D7'.
        Use 'D7x' or 'D7y' to indicate which axis to use for the 180 degree
        rotation.

        Adapted from commons.geometry.symmetry.py.
        """

        if name[0] == 'D':
            n = int(name[1])
            # print 'Generating D%d symmetry group.' % n
            if len(name) > 2:
                axis_name = name[2]
            else:
                axis_name = 'y'
            # print(('Small rotations about the z-axis and '
            #       '180 degree rotation about the ' +
            #       '%s axis.') % axis_name)

            angle = 2.0 * np.pi / n
            c = np.cos(angle)
            s = np.sin(angle)
            a = np.array([ [c, -s, 0.0],
                           [s, c, 0.0],
                           [0.0, 0.0, 1.0] ])
            b = np.eye(3)
            # 180 degree rotation about the y-axis
            b[2,2] = -1.0
            if axis_name == 'y':
                b[0,0] = -1.0
            else: # Assume axis_name == 'x'
                b[1,1] = -1.0

            current = np.eye(3)
            symmetries = []
            for i in range(n):
                symmetries.append(Rotation3D(current.copy()))
                symmetries.append(Rotation3D(np.dot(current, b)))
                current = np.dot(current, a)

            return symmetries
        else:
            print 'Rotation group %s not supported.' % name
            return []

    cpdef copy_in_place(self, Rotation3D other):
        cdef:
            int i, j
        
        for i in range(3):
            for j in range(3):
                other._matrix[i, j] = self._matrix[i, j]
        
    cpdef compose_in_place(self, Rotation3D other, Rotation3D result):
        """Set result to other followed by this."""

        multiply3x3(self._matrix, other._matrix, result._matrix)



cdef class Rotation2D(Rotation):
    """Rotation of 2D vectors or 2D object.

    By convention this is the rotation of the object, not the frame.
    In the matrix representation, the two conventions are related
    through the transpose operation.
    """

    @classmethod
    def random(cls):
        """Constructs a random rotation."""

        theta = np.random.random() * np.pi * 2.0

        return Rotation2D.from_angle(theta)

    @classmethod
    def from_angle(cls, angle):
        """Return anti-clockwise rotation by 'angle' radians."""

        c = np.cos(angle)
        s = np.sin(angle)

        matrix = np.array([ [c, -s],
                            [s, c] ])

        return cls(matrix)

    def to_angle(self):
        """Return angle representation of this rotation."""

        c = self.matrix[0, 0]
        s = self.matrix[1, 0]

        return np.arctan2(s, c)

    def invert(self):
        """Return rotation with transpose of matrix."""

        return Rotation2D(np.array(self._matrix).T)


@cython.boundscheck(False)
@cython.cdivision(True)
@cython.wraparound(False)
cdef class ExpTrace(Distribution):
    """Distribution over rotations.

    Rotation R distributed according to exp(trace(dot(A.T, R))).

    Originally copied from CSB. (csb.statistics.rand.py)
    Implements algorithm from Habeck, 2009.
    """

    def __cinit__(self, int n_iter=10):
        self._n_iter = n_iter

        self.U = np.empty([3, 3], dtype='double')
        self.L = np.empty(3, dtype='double')
        self.V = np.empty([3, 3], dtype='double')
        self.rotation = np.empty((3, 3), dtype='double')
        self.rotation2 = np.empty((3, 3), dtype='double')
        self.rotation3 = np.empty((3, 3), dtype='double')

    cdef double clip(self, double x, double a, double b):
        if x < a:
            return a
        if x > b:
            return b
        return x

    cdef double det(self, double[:, :] x):
        """Returns the determinant of 3x3 matrix x."""

        return (x[0, 0] * (x[1, 1] * x[2, 2] - x[1, 2] * x[2, 1]) -
                x[0, 1] * (x[1, 0] * x[2, 2] - x[2, 0] * x[1, 2]) +
                x[0, 2] * (x[1, 0] * x[2, 1] - x[2, 0] * x[1, 1]))


    cdef double _sample_beta(self, double kappa):

        cdef:
            double u, x

        u = gsl_ran_flat(r, 0, 1)

        if kappa <> 0.:
            x = self.clip(
                1 + 2 * log(u + (1-u) * exp(-kappa)) / kappa,-1.,1.)
        else:
            x = 2*u - 1

        return acos(x)


    cpdef double[:, :] sample(self, double[:, ::1] A):
        """Returns a random rotation."""

        cdef:
            int i
            double[:, :] result
            double beta, phi, psi, constant, alpha, gamma, kappa

        svd3x3(A, self.U, self.L, self.V)

        beta = 0.0

        for i in range(self._n_iter):

            ## sample alpha and gamma
            phi = vonmisesvariate(
                0., self.clip(cos(beta/2)**2 * (self.L[0]+self.L[1]),
                              1e-308, 1e10))
            psi = vonmisesvariate(M_PI, sin(beta/2)**2 *
                                  (self.L[0]-self.L[1]))

            ## sample beta
            kappa = (cos(phi) * (self.L[0]+self.L[1]) + cos(psi) *
                     (self.L[0]-self.L[1]) + 2 * self.L[2])
            beta  = self._sample_beta(kappa)

        ut = gsl_ran_flat(r, 0, 1)
        constant = 0 if ut < 0.5 else M_PI

        alpha = 0.5 * (phi + psi) + constant
        gamma = 0.5 * (phi - psi) + constant

        euler_angles_to_rotation(alpha, beta, gamma, self.rotation)

        dot3x3(self.V, self.rotation, self.rotation2)
        transpose3x3(self.rotation2, self.rotation3)
        result = np.empty((3, 3), dtype='double')
        dot3x3(self.U, self.rotation3, result)

        return result


cdef class ExpTrace2D(Distribution):
    """Distribution over 2D rotations.

    Rotation R distributed according to exp(trace(dot(A.T, R))).

    This is just the von Mises distribution using a different
    parametrisation.
    """

    def __cinit__(self):
        pass

    cpdef double sample(self, double[:, :] A):
        """Returns a random 2D rotation.

        Args:
            A: 2x2 matrix.

        Returns:
            Angle in radians describing counterclockwise rotation.
        """

        # Convert A to more familiar von Mises parameters.
        a = A[1, 0] - A[0, 1]
        b = A[0, 0] + A[1, 1]

        mu = np.arctan2(a, b)
        K = np.sqrt(a*a + b*b)

        angle = np.random.vonmises(mu, K)

        return angle




cdef euler_angles_to_rotation(
    double alpha, double beta, double gamma, double[:, :] matrix):
    """Converts euler angles to rotation.

    This is the same as the static constructor in Rotation3D.
    """

    cdef:
        double c1, c2, c3, s1, s2, s3

    c1 = cos(alpha)
    c2 = cos(beta)
    c3 = cos(gamma)
    s1 = sin(alpha)
    s2 = sin(beta)
    s3 = sin(gamma)

    matrix[0, 0] = c1 * c2 * c3 - s1 * s3
    matrix[0, 1] = c1 * s3 + c2 * c3 * s1
    matrix[0, 2] = -c3 * s2
    matrix[1, 0] = -c3 * s1 - c1 * c2 * s3
    matrix[1, 1] = c1 * c3 - c2 * s1 * s3
    matrix[1, 2] = s2 * s3
    matrix[2, 0] = c1 * s2
    matrix[2, 1] = s1 * s2
    matrix[2, 2] = c2

cpdef exponential_coordinates_to_rotation(double[:] v,
                                         double[:, :] matrix):
    """Convert rotation representation to matrix.

    Equivalent to:
    axis = v / np.linalg.norm(v)
    angle = np.linalg.norm(v)
    matrix = Rotation3D.from_axis_angle(axis, angle)._matrix

    Args:
        v: (3, ) array, rotation in exponential coordinates
        matrix: (3, 3), will contain result
    """
    
    cdef:
        double angle, t, cs, sn, a, b, c, d

    # First convert to quaternion.
    angle = norm3d(v)
    t = 0.5 * angle
    cs = cos(t)
    sn = sin(t)

    if angle < 1e-9:
        a, b, c, d = 1.0, 0.0, 0.0, 0.0
    else:
        a = cs
        b = sn * v[0] / angle
        c = sn * v[1] / angle
        d = sn * v[2] / angle

    # Then convert from quaternion to matrix.
    matrix[0, 0] = a*a + b*b - c*c - d*d
    matrix[0, 1] = 2*b*c - 2*a*d        
    matrix[0, 2] = 2*b*d + 2*a*c        
    matrix[1, 0] = 2*b*c + 2*a*d        
    matrix[1, 1] = a*a - b*b + c*c - d*d
    matrix[1, 2] = 2*c*d - 2*a*b        
    matrix[2, 0] = 2*b*d - 2*a*c        
    matrix[2, 1] = 2*c*d + 2*a*b        
    matrix[2, 2] = a*a - b*b - c*c + d*d

    

cdef dot3x3(double[:, :] x, double[:, :] y, double[:, :] result):
    """Computes the product of 2 3x3 matrices."""

    cdef:
        int i, j

    for i in range(3):
        for j in range(3):
            result[i, j] = (x[i, 0] * y[0, j] +
                            x[i, 1] * y[1, j] +
                            x[i, 2] * y[2, j])

cdef transpose3x3(double[:, :] x, double[:, :] result):
    """Return x.T"""

    cdef:
        int i, j

    for i in range(3):
        for j in range(3):
            result[i, j] = x[j, i]



cdef class UniformRotations:
    """For generating sets of uniform rotations.

    Can be either random or deterministic.
    """

    def __cinit__(self, num_rotations):
        self._num_rotations = num_rotations

    cpdef double[:, ::1] generate_random(self):
        """Return uniformly distributed, random rotations."""

        cdef:
            double[:, ::1] rotations, matrix
            double alpha, cosbeta, beta, gamma
            int i, j, k

        rotations = np.empty((self._num_rotations, 9), dtype='double')
        matrix = np.empty((3, 3), dtype='double')

        for i in range(self._num_rotations):

            # Generate random Euler angles.
            alpha = TWOPI * gsl_ran_flat(r, 0, 1)
            cosbeta = 2.0 * gsl_ran_flat(r, 0, 1) - 1.0
            beta = acos(cosbeta)
            gamma = TWOPI * gsl_ran_flat(r, 0, 1)

            # Convert to rotation matrix.
            euler_angles_to_rotation(alpha, beta, gamma, matrix)
            for j in range(3):
                for k in range(3):
                    rotations[i, 3*j + k] = matrix[j, k]

        return rotations

    cpdef double[:, ::1] generate_variable_stepping(self):
        """Return grid of rotations using the variable stepping method.

        From J. Mitchell, Sampling rotation groups by successive
        orthogonal images, 2008, p534.
        """

        cdef:
            double step, step2, alpha, beta, gamma, den
            int N, N_max, i, j, k
            int alpha_index, beta_index, gamma_index
            double[:, ::1] rotations, rotations2, matrix

        # Find optimal angular step size.
        N = self._num_rotations
        step = pow(8.0 * np.pi * np.pi / N, 1.0 / 3.0)

        # Find an upper bound on the final number of rotations:
        N_max = 10 * N

        # Generate grid.
        rotations = np.empty((N_max, 9), dtype='double')
        matrix = np.empty((3, 3), dtype='double')
        i = 0
        num_betas = np.floor(np.pi / step) + 1
        num_gammas = np.floor(2.0 * np.pi / step) + 1
        for beta_index in range(num_betas):
            beta = beta_index * step

            den = sin(beta)
            if den == 0.0:
                num_alphas = 1
            else:
                step2 = step / den
                num_alphas = np.floor(2.0 * np.pi / step2) + 1
            for alpha_index in range(num_alphas):
                alpha = alpha_index * step
                for gamma_index in range(num_gammas):
                    gamma = gamma_index * step

                    # Convert to rotation matrix.
                    euler_angles_to_rotation(alpha, beta, gamma, matrix)
                    for j in range(3):
                        for k in range(3):
                            rotations[i, 3*j + k] = matrix[j, k]
                    i += 1

        # Copy relevant rotations to new memory.
        N_true = i
        rotations2 = np.empty((N_true, 9), dtype='double')
        for i in range(N_true):
            for j in range(9):
                rotations2[i, j] = rotations[i, j]

        return rotations2


cdef class RotationGradients:
    """Compute gradients of rotation w.r.t. exponential coordinates.

    From Gallego & Yezzi, 2013, Eqn III.7.

    All memory assigned upon initialisation.
    """

    def __cinit__(self):

        self._identity = np.eye(3)
        self._term1 = np.empty((3, 3), dtype='double')        
        self._term2 = np.empty((3, 3), dtype='double')
        self._vector = np.empty((3, ), dtype='double')
        self._R = np.empty((3, 3), dtype='double')
        self._dvx = np.empty((3, 3), dtype='double')
        self._dvy = np.empty((3, 3), dtype='double')
        self._dvz = np.empty((3, 3), dtype='double')        

    cpdef get_dvx(self):
        return self._dvx

    cpdef get_dvy(self):
        return self._dvy

    cpdef get_dvz(self):
        return self._dvz

    cpdef get_dv(self, int index):
        if index == 0:
            return self._dvx
        if index == 1:
            return self._dvy
        if index == 2:
            return self._dvz

    cpdef compute(self, double[:] v):
        """Compute all derivatives (3 3x3 matrices).

        Args:
            v: (3, ) array with exponential coordinates
        """

        cdef:
            int i
            double n2
            double[:] column

        n2 = norm_squared3d(v)
        if n2 < 1e-9:
            # Rotation is the identity.
            self._dvx = np.array([[0.0, 0.0, 0.0],
                                  [0.0, 0.0, -1.0],
                                  [0.0, 1.0, 0.0]])
            self._dvy = np.array([[0.0, 0.0, 1.0],
                                  [0.0, 0.0, 0.0],
                                  [-1.0, 0.0, 0.0]])
            self._dvz = np.array([[0.0, -1.0, 0.0],
                                  [1.0, 0.0, 0.0],
                                  [0.0, 0.0, 0.0]])
        else:

            # Convert exponential coordinates to rotation matrix.
            exponential_coordinates_to_rotation(v, self._R)



            for i in range(3):
                # Compute second term.
                subtract3x3(self._identity, self._R, self._term2)
                column = self._term2[:, i]
                cross(v, column, self._vector)
                to_skew_symmetric(self._vector, self._term2)

                # Compute first term.
                to_skew_symmetric(v, self._term1)
                scale3x3(self._term1, v[i])

                # Combine terms.
                add3x3(self._term1, self._term2, self._term1)
                scale3x3(self._term1, 1.0 / norm_squared3d(v))

                # Final multiplication.
                multiply3x3(self._term1, self._R, self._term2)
                if i == 0:
                    copy3x3(self._term2, self._dvx)
                if i == 1:
                    copy3x3(self._term2, self._dvy)
                if i == 2:
                    copy3x3(self._term2, self._dvz)

    cpdef scalar_derivative(self, double[:, :] dEdR, double[:] dEdv):
        """Compute dE/dv, a (3, ) vector.

        Should run self.compute before this function.

        Args:
            dEdR : (3, 3), derivative of energy w.r.t. rotation matrix.
            dEdv : (3, ), derivative of energy w.r.t. v, results
        """
        
        cdef int i
        
        for i in range(3):
            dEdv[i] = hadamard3x3(dEdR, self.get_dv(i))

    cpdef relative_scalar_derivative(
        self, double[:, :] dEdR, double[:, :] R0,
        double[:] dEdv):
        """Compute dE/dv, a (3, ) vector.

        Should run self.compute before this function.

        Args:
            dEdR : (3, 3), derivative of energy w.r.t R.
            R0 : (3, 3) rotation matrix. R = R(v)*R0
            dEdv : (3, ), derivative of energy w.r.t. v, results
        """
        
        cdef int i
        
        for i in range(3):
            multiply3x3(self.get_dv(i), R0, self._R)
            dEdv[i] = hadamard3x3(dEdR, self._R)
            # dEdv[i] = hadamard3x3(dEdR, self.get_dv(i))
        

