import numpy as np
import simulation_helpers as sh

#
# This is a simple implementation I have looked up in the wiki
# THIS ONE is used by Turnable class currently
#
def make_rotation_matrix( nn, a ):
    """ return rotation matrix for angle a around nn """
    R = np.zeros( (3,3) )
    R[0,0] = nn[0]*nn[0] * (1-np.cos(a)) + np.cos(a)
    R[1,0] = nn[0]*nn[1] * (1-np.cos(a)) + nn[2]*np.sin(a)
    R[2,0] = nn[0]*nn[2] * (1-np.cos(a)) - nn[1]*np.sin(a)
    
    R[0,1] = nn[0]*nn[1] * (1-np.cos(a)) - nn[2]*np.sin(a)
    R[1,1] = nn[1]*nn[1] * (1-np.cos(a)) + np.cos(a)
    R[2,1] = nn[1]*nn[2] * (1-np.cos(a)) + nn[0]*np.sin(a)
    
    R[0,2] = nn[0]*nn[2] * (1-np.cos(a)) + nn[1]*np.sin(a)
    R[1,2] = nn[1]*nn[2] * (1-np.cos(a)) - nn[0]*np.sin(a)
    R[2,2] = nn[2]*nn[2] * (1-np.cos(a)) + np.cos(a)
        
    return R

#
# This is a smarter implementation I have looked up on the web somewhere
# I have timed it and it seems not to be faster then the upper one
# However the returned rotation matrices of both differ by a sign
# in two off axis elements... not sure what this means :-|
#
def make_axis_rotation_matrix(direction, angle):
    """
    Create a rotation matrix corresponding to the rotation around a general
    axis by a specified angle.
    R = dd^T + cos(a) (I - dd^T) + sin(a) skew(d)
    Parameters:
        angle : float a
        direction : array d
    """
    d = np.array(direction, dtype=np.float64)
    d /= np.linalg.norm(d)
    eye = np.eye(3, dtype=np.float64)
    ddt = np.outer(d, d)
    skew = np.array([[    0,  d[2],  -d[1]],
                     [-d[2],     0,  d[0]],
                     [d[1], -d[0],    0]], dtype=np.float64)
    mtx = ddt + np.cos(angle) * (eye - ddt) + np.sin(angle) * skew
    return mtx


class Turnable( object ):
    """ *Virtual* class for inheritance of turn() method only
    
    turn( axis, angle) - turn any member of class, which is
    contained in list-like self._turnables.
    
    sub-class constructor should fill self._turnables
    """
    def turn(self, axis, angle):
        """ turn all members in self.turnables around *axis* by angle
            
            *axis* : can be any 3-element np.array or np.array - constructable
                    like e.g. a list 
            *angle* : in radians
        """
        
        axis = np.array(axis)
        
        if sh.length(axis) != 1.:
            axis /= sh.length(axis)
        
        R = make_rotation_matrix( axis, angle )
        
        for turnable in self._turnables:
            if hasattr(self, turnable):
                setattr( self, turnable,  np.dot(R, getattr( self, turnable)) )
    
    # standard __repr__
    def __repr__( self ):
        return "%s(%r)" % (self.__class__, self.__dict__)