"""
     pygl/Quat.py: CCP4MG Molecular Graphics Program
     Copyright (C) 2001-2005 University of York, CCLRC

     This library is free software: you can redistribute it and/or
     modify it under the terms of the GNU Lesser General Public License
     version 3, modified in accordance with the provisions of the 
     license to address the requirements of UK law.
 
     You should have received a copy of the modified GNU Lesser General 
     Public License along with this library.  If not, copies may be 
     downloaded from http://www.ccp4.ac.uk/ccp4license.php
 
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU Lesser General Public License for more details.
"""


from math import *
import util

PI = 3.141592653589793238462643
RAD_TO_DEG = 180.0 / PI
DEG_TO_RAD = PI / 180

class Quat:
     """
     Quaternion class
     """
     def rotate_about_axes(self,xaxis,yaxis,zaxis,drot):
        rotx = Quat(xaxis[0],xaxis[1],xaxis[2],1,drot[0])
        roty = Quat(yaxis[0],yaxis[1],yaxis[2],1,drot[1])
        rotz = Quat(zaxis[0],zaxis[1],zaxis[2],1,drot[2])
        self.postMult(rotx)
        self.postMult(roty)
        self.postMult(rotz)

     def  __init__(self,x=0,y=0,z=0,wi=-1,angle=0):
        """
        Constructor, specify wi=0 for build with x,y,z and
        wi = 1 for x,y,z,angle
        """

        self.dval = [ 1.0, 0.0, 0.0, 0.0 ]

        if wi == 0:
            self.set(x,y,z)
        if wi == 1:
            self.seta(angle,x,y,z)
       
     def reset(self):
        """
        Reset to unit rotation.
        """
        self.dval = [ 1.0, 0.0, 0.0, 0.0 ]

     def set(self,x,y,z):
        """
        Set to given rotations about x,y and z axes.
        """

        xQ = Quat(1.0, 0.0, 0.0, wi=1, angle=x)
        yQ = Quat(0.0, 1.0, 0.0, wi=1, angle=y)
        zQ = Quat(0.0, 0.0, 1.0, wi=1, angle=z)

        self.setq(xQ)
        self.postMult(yQ)
        self.postMult(zQ)

     def setq(self,qfrom):
        """
        Copy dval from other quaternion. This
        should be used where we currently have
        dodginess in MG.
        """
        self.dval = qfrom.dval

     def seta(self,angle,x,y,z):
        """
        Set rotation angle about 
        specified axis.
        """

        angle = angle * DEG_TO_RAD

        radius = sqrt(x*x + y*y +z*z)
        if radius < 0.000000001:
           print "zero length in Quat norm"
        x = x / radius
        y = y / radius
        z = z / radius

        self.dval[0] = cos(angle/2.0)
        self.dval[1] = x*sin(angle/2.0)
        self.dval[2] = y*sin(angle/2.0)
        self.dval[3] = z*sin(angle/2.0)

     def postMult(self,q):
        """
        Post-multiply by another quaternion.
        """
        temp = Quat()
        temp.dval = []
        temp.dval.extend(self.dval)
        self.multAndSet(temp,q)

     def multAndSet(self,quat1,quat2):
        """
        Set self to be product of quat1 and quat2.
        """
        self.dval[0] = quat2.dval[0] * quat1.dval[0] - quat2.dval[1] * quat1.dval[1] \
                                                     - quat2.dval[2] * quat1.dval[2] \
                                                     - quat2.dval[3] * quat1.dval[3]

        self.dval[1] = quat2.dval[0] * quat1.dval[1] + quat2.dval[1] * quat1.dval[0] \
                                                     + quat2.dval[2] * quat1.dval[3] \
                                                     - quat2.dval[3] * quat1.dval[2]

        self.dval[2] = quat2.dval[0] * quat1.dval[2] - quat2.dval[1] * quat1.dval[3] \
                                                     + quat2.dval[2] * quat1.dval[0] \
                                                     + quat2.dval[3] * quat1.dval[1]

        self.dval[3] = quat2.dval[0] * quat1.dval[3] + quat2.dval[1] * quat1.dval[2] \
                                                     - quat2.dval[2] * quat1.dval[1] \
                                                     + quat2.dval[3] * quat1.dval[0]
     def normalize(self):
        """
        Normailze the dvals.
        """
        self.dval = util.normalize(self.dval)

     def getInvMatrix(self):
        self.normalize()

        quat = Quat()
        quat.dval = []
        quat.dval.append(self.dval[0])
        quat.dval.append(-self.dval[1])
        quat.dval.append(-self.dval[2])
        quat.dval.append(-self.dval[3])
        return quat.getMatrix()

     def getInvMatrix_c(self):
        self.normalize()

        quat = Quat()
        quat.dval = []
        quat.dval.append(self.dval[0])
        quat.dval.append(-self.dval[1])
        quat.dval.append(-self.dval[2])
        quat.dval.append(-self.dval[3])
        return quat.getMatrix_c()

     def getMatrix(self):
        """
        Return the rotation matrix.
        """

        self.normalize()

        w = self.dval[0]
        x = self.dval[1]
        y = self.dval[2]
        z = self.dval[3]

        xx = x*x
        yy = y*y
        zz = z*z

        M = [ [], [], [], [] ]

        M[0].append( 1.0 - 2.0 * ( yy + zz ))
        M[1].append( 2.0 * ( x * y + w * z ))
        M[2].append( 2.0 * ( x * z - w * y ))
        M[3].append( 0.0)

        M[0].append(  2.0 * ( x * y - w * z ))
        M[1].append(  1.0 - 2.0 * ( xx + zz ))
        M[2].append(  2.0 * ( y * z + w * x ))
        M[3].append(  0.0)

        M[0].append(  2.0 * ( x * z + w * y ))
        M[1].append(  2.0 * ( y * z - w * x ))
        M[2].append(  1.0 - 2.0 * ( xx + yy ))
        M[3].append(  0.0)

        M[0].append(  0.0)
        M[1].append(  0.0)
        M[2].append(  0.0)
        M[3].append(  1.0)

        return M

     def getMatrix_c(self):
        import point_funcs
        cmat = point_funcs.doublea(16)
        M = self.getMatrix()
        cmat[0] = M[0][0]
        cmat[1] = M[1][0]
        cmat[2] = M[2][0]
        cmat[3] = M[3][0]
        cmat[4] = M[0][1]
        cmat[5] = M[1][1]
        cmat[6] = M[2][1]
        cmat[7] = M[3][1]
        cmat[8] = M[0][2]
        cmat[9] = M[1][2]
        cmat[10] = M[2][2]
        cmat[11] = M[3][2]
        cmat[12] = M[0][3]
        cmat[13] = M[1][3]
        cmat[14] = M[2][3]
        cmat[15] = M[3][3]
        return cmat