import math
import vec3,mat3,mat4

class Quaternion:
    """
    Quaternions
    -----------
    An extension to complex numbers
    Can be used to store rotations

    """
    
    def __init__(self,*args):
        """
        Initialize the object

        0 arguments -
            set all values to 0
        1 arguments -
            1 float or 1 int : assign arg to w , set others to 0
            1 quaternion object : make a copy
        2 arguments -
            angle and axis
        4 arguments -
            assing args to w,x,y,z
        
        """

        # 0 arguments
        if len(args) == 0:
            # set all to 0
            self.w,self.x,self.y,self.z = (0.0,0.0,0.0,0.0)

        # 1 argument
        elif len(args) == 1:
            
            # if its a float
            if type(args[0]) == float:
                self.w = float(args[0])
                self.x , self.y , self.z = 0.0,0.0,0.0
                
            # if its an instance of Quaternion class
            elif isinstance(args[0],Quaternion):
                q = args[0]
                self.w = q.w
                self.x = q.x
                self.y = q.y
                self.z = q.z

            # if its a string
            elif type(args[0]) == str:
                values = args[0]
                values = list(values.split(','))
                
                self.w = float(values[0])
                self.x = float(values[1])
                self.y = float(values[2])
                self.z = float(values[3])
                
            # list or tuple
            elif type(args[0]) == list or type(args[0]) == tuple:
                self.w,self.x,self.y,self.z = args[0]
            else:
                print(args[0])
            
        # 2 arguments
        elif len(args) == 2:
            angle,axis = args
            self.fromAngleAxis(angle,axis)

        # 4 arguments
        elif len(args) == 4:
            self.w , self.x , self.y , self.z = args
        else:
            raise TypeError


    def __eq__(self,other):
        """
        == operator
        """
        if isinstance(other,Quaternion):
            return (self.w == other.w and self.x == other.x and
                    self.y == other.y and self.z == other.z )
        else:
            return 0

    def show(self):
        """ print info """
        print('w : '+str(self.w)+
              'x : '+str(self.x)+
              'y : '+str(self.y)+
              'z : '+str(self.z)
             )

    def __ne__(self,other):
        """
        != operator
        """
        if isinstance(other,Quaternion):
            return (self.w != other.w or self.x != other.x or
                    self.y != other.y or self.z != other.z)
        else:
            return 0


    def __add__(self,other):
        """
        Addition :
        
        - Adds one quaternion to another
        
        """
        if isinstance(other,Quaternion):
            return Quaternion(self.w+other.w,
                              self.x+other.x,
                              self.y+other.y,
                              self.z+other.z)
        else:
            raise TypeError


    def __sub__(self,other):
        """
        Subtraction :
        
        - Subtract one quaternion from another
        
        """
        if isinstance(other,Quaternion):
            return Quaternion(self.w-other.w,
                              self.x-other.x,
                              self.y-other.y,
                              self.z-other.z)
        else:
            raise TypeError


    def __mul__(self,other):
        """
        Multiplication :
        
        - Multiply one quaternion with another

        """
        if type(other) ==float or type(other) == int:
            return Quaternion(self.w*other,self.x*other,
                              self.y*other,self.z*other)

        if isinstance(other,Quaternion):
            w1,x1,y1,z1 = self.w,self.x,self.y,self.z
            w2,x2,y2,z2 = other.w,other.x,other.y,other.z
            
            return Quaternion(w1*w2 - x1*x2 - y1*y2 - z1*z2,
                              w1*w2 + x1*x2 + y1*y2 - z1*z2,
                              w1*w2 + x1*x2 - y1*y2 + z1*z2,
                              w1*w2 + x1*x2 + y1*y2 - z1*z2,)

        
    def __truediv__(self,other):
        """
        Division
        
        - Divide one quaternion to another

        """        
        if type(other) == int or type(other) == float:
            return Quaternion(self.w / other,
                              self.x / other,
                              self.y / other,
                              self.z / other)


    def __neg__(self):
        """
        Negation
        """
        return Quaternion(-self.w,-self.x,-self.y,-self.z)


    def __pos__(self):
        """
        pos
        """
        return Quaternion(+self.w,+self.x,+self.y,+self.z)

    def length(self,euclidian=False):
        """
        returns the euclidian length
        """
        if euclidian == True:
            # return euclidian length
            return float(math.sqrt( self.w*self.w + self.x*self.x +
                                    self.y*self.y + self.z*self.z ))
        else:
            # return normal length
            return (self.w * self.w,
                    self.x * self.x,
                    self.y * self.y,
                    self.z * self.z)

    def __abs__(self):
        """
        magnitude
        """
        return math.sqrt(self.w*self.w + self.x*self.x +
                         self.y*self.y + self.z*self.z)


    def conjugate(self):
        """
        Returns conjugate
        """
        return Quaternion(self.w,-self.x,-self.y,-self.z)

    def normalize(self):
        """
        Normalized quaternion
        """
        leng = 1.0 / abs(self)
        return Quaternion( self.w*leng, self.x*leng,
                           self.y*leng, self.z*leng)

    def inverse(self):
        """
        Returns inverse
        """
        length = self.w*self.w + self.x*self.x + self.y*self.y + self.z*self.z
        return self.conjugate()/length

    def toAngleAxis(self):
        normalself = self.normalize()

        w = max(min(normalself.w,1.0),-1.0)
        
        w = math.acos(w)
        s = math.sin(w)
        if s<1E-12:
            return (0.0, vec3())
        return (2.0*w, vec3(normalself.x/s, normalself.y/s, normalself.z/s))

    def fromAngleAxis(self, angle, axis):
        """Initialize self from an angle (in radians) and an axis and returns self."""
        angle/=2.0
        self.w = math.cos(angle)
        x, y, z = axis.x,axis.y,axis.z
        s = math.sin(angle)/math.sqrt(x*x+y*y+z*z)
        self.x=x*s
        self.y=y*s
        self.z=z*s

        return self

    def toMat3(self):
        """Return rotation matrix as mat3."""
        x,y,z,w = self.x, self.y, self.z, self.w
        xx = 2.0*x*x
        yy = 2.0*y*y
        zz = 2.0*z*z
        xy = 2.0*x*y
        zw = 2.0*z*w
        xz = 2.0*x*z
        yw = 2.0*y*w
        yz = 2.0*y*z
        xw = 2.0*x*w
        return mat3(1.0-yy-zz, xy-zw, xz+yw,
                    xy+zw, 1.0-xx-zz, yz-xw,
                    xz-yw, yz+xw, 1.0-xx-yy)

    def toMat4(self):
        """
        Return rotation matrix as mat4.
        """
        x,y,z,w = self.x, self.y, self.z, self.w
        xx = 2.0*x*x
        yy = 2.0*y*y
        zz = 2.0*z*z
        xy = 2.0*x*y
        zw = 2.0*z*w
        xz = 2.0*x*z
        yw = 2.0*y*w
        yz = 2.0*y*z
        xw = 2.0*x*w
        return mat4(1.0-yy-zz, xy-zw, xz+yw, 0.0,
                    xy+zw, 1.0-xx-zz, yz-xw, 0.0,
                    xz-yw, yz+xw, 1.0-xx-yy, 0.0,
                    0.0, 0.0, 0.0, 1.0)

    def fromMat(self, m):
        """Initialize self from either a mat3 or mat4 and returns self."""
        
        d1,d2,d3 = m[0,0],m[1,1],m[2,2]
        t = d1+d2+d3+1.0
        if t>0.0:
            s = 0.5/math.sqrt(t)
            self.w = 0.25/s
            self.x = (m[2,1]-m[1,2])*s
            self.y = (m[0,2]-m[2,0])*s
            self.z = (m[1,0]-m[0,1])*s
        else:
            ad1 = abs(d1)
            ad2 = abs(d2)
            ad3 = abs(d3)
            if ad1>=ad2 and ad1>=ad3:
                s = math.sqrt(1.0+d1-d2-d3)*2.0
                self.x = 0.5/s
                self.y = (m[0,1]+m[1,0])/s
                self.z = (m[0,2]+m[2,0])/s
                self.w = (m[1,2]+m[2,1])/s
            elif ad2>=ad1 and ad2>=ad3:
                s = math.sqrt(1.0+d2-d1-d3)*2.0
                self.x = (m[0,1]+m[1,0])/s
                self.y = 0.5/s
                self.z = (m[1,2]+m[2,1])/s
                self.w = (m[0,2]+m[2,0])/s
            else:
                s = math.sqrt(1.0+d3-d1-d2)*2.0
                self.x = (m[0,2]+m[2,0])/s
                self.y = (m[1,2]+m[2,1])/s
                self.z = 0.5/s
                self.w = (m[0,1]+m[1,0])/s

        return self
