import math
import numpy
import LinearAlgebra
import Matrix

from array import *

# system stuff
import os
import copy

# pretty printing
import pprint

# for importing as a plugin into PyMol
#import tkSimpleDialog
#import tkMessageBox
from pymol import cmd
from pymol import stored
from pymol import selector

class kabsch:
	"""
	Kabsch alignment of two set of vectors to produce and optimal alignment.

	Steps
	=====	
		1.  Calculate the center of mass for each protein.  Then, move the protein
		to its center of mass.  We choose as a convention, to use the origin as 
		the center of mass of the first set of coordinates.  This will allow us to
		return one translation vector, instead of two.
		
		Update: Since any rotation around a point that's not the origin, is in fact
		an affine rotation.  So, to beat that, we translate both to the center.
		
		NAME: superpose(c1, c2)
		POST: T is now defined.
		
		2.  Calculate the matrix, R, by (eq7, 1976).  r_{i,j} = sum_n w_n * y_{ni} * x_{nj},
		where y_{ni} is the ith component of the vector y_n.
		NAME: calcR
		POST: R is now defined
		
		3.  Calculate RtR (R-transpose * R).
		NAME: calcRtR
		POST: RtR is now defined
		
		4.  Calculate the corresponding eigenpairs for RtR, and sort them accordingly:
		m1 >= m2 >= m3; set v3 = v1 x v2 to ensure a RHS
		NAME: calcEigenPairs
		POST: The eigen pairs are calculated, sorted such that m1 >= m2 >= m3 and
		v3 = v1 x v2.
		
		5.  Calculate R*v_k and normalize the first two vectors to obtain b_1, b_2 and set
		b_3 = b_1 x b_2.  This also takes care of m1 > m2 = 0. 
		NAME: calcBVectors
		POST: b-Vectors are defined and returned
		
		6.  Calculate U=(u_{ij})=(sum n b_{ki} * a_{kj}) to obtain the best rotation.  Set
		sigma_3 = -1 if b3(Ra3) < 0 else sigma_3 = +1.
		NAME: calcU
		POST: U is defined
		
		7.  Calculate the RMSD.  The residual error is then
		The E = E0 - sqrt(m1) - sqrt(m2) - sigma_3(sqrt(m3)).
		NAME: calcRMSD
		POST: RMSD is computed.
	
	 @note: This should be a static method that takes three parameters
		
		1. The first protein's coordinates, f.  This program will 
		accept coordinates in the form an array of 3D vectors/lists
		
		2. The second protein's coordinates, g.
		
		3. The array of integer pairs representing the pairs to align.
		Coordinates should be formatted as as array of 2D vectors/lists.
	

	
	"""
	
	def __init__(self):
		"""
		Constructor.  @see kabsch.align.
		
		"""

		#
		# Instance Variables:  All three of these will be updated
		# every time the user calls ~.align.  Just to warn ya'.
		#		
		# U, the rotation matrix
		self.U = []
		# T, the translation vector
		self.T = []
		# R, the RMSD
		self.R = -1.0

		#self.menuBar.addmenuitem('Plugin', 'command', 'Kabsch Align', label = "Align Selections to Optial RMSD", command = lamda s=self: fetchPDBDialog(s))
		
				
	def align(self, c1, c2, pairs):
		"""
		Finds the best alignment of c1 and c2's pairs resulting in
		the smallest possible RMSD.

				
		@note:
			- All weights in this first version are set to 1.  Kabsch allows,
			differential weighting.  In the future, I may extend to this option,
			and then pairs may become 3D (r1, r2, weight) or I may add another
			parameter.
		
			- Helper functions will soon be provided such that the user may
			just use this package alone to compute the rotation.
		
		@param c1: coordinats of the first vectors, as an array of 3D vectors.
		@type  c1: Python list
		   
		@param c2: coordinates of the second set of vectors, as an array of 3D vectors.
		@type  c2: Python list
		   
		@param pairs: the list of pairs as an array of 2D pairs.
		@type  pairs: Python list of 2D lists.
		
		@return: U, the rotation matrix that gives the optimal rotation between the two proteins.
			
			T, the translation vector given to align the two objects centers of mass.
			
			R, the RMSD of the final rotation.
		"""
		
		#
		# First we move the center of mass of one protein, to the
		# center of mass of the other.  This removes any translation
		# between the two.
		#
		T1, T2, c1, c2 = self.superpose(c1, c2)
		# Calculate the initial RMSD
		E0 = self.calcE0(c1, c2)
		# Calculate R via eq. 7.
		R = self.calcR(c1, c2)
		# Calculate R(transpose)*R
		RtR = self.calcRtR(R)
		# Determined the eigenpairs for the matrix RtR.
		eValues, eVectors = self.calcEigenPairs(RtR)
		# Determine the bVectors as required
		bVectors = self.calcBVectors(R, eVectors)
		# Calculate the roation matrix
		U = self.calcU(eVectors, bVectors)
		# Calculate the final RMSD using U.
		RMSD = self.calcRMSD(E0, eValues, eVectors, bVectors, R, len(c1))
		
		return U, T1, T2, RMSD, c1, c2
		
		
	def superpose(self, c1, c2 ):
		"""
		Calculate the center of mass for each protein.  Then, move the protein
		to its center of mass.  We choose as a convention, to use the origin as 
		the center of mass of the first set of coordinates.  This will allow us to
		return one translation vector, instead of two.
		(CORRECT)
		
		@precondition: c1 and c2 are well defined lists of N-dimensional points with length > 0.
		@postcondition: T is now defined.
		
		@param c1: coordinats of the first vectors, as an array of 3D vectors.
		@type  c1: Python list
		   
		@param c2: coordinates of the second set of vectors, as an array of 3D vectors.
		@type  c2: Python list
		
		@return: T the translation vector.
		
			c2 one list of coordinates that of c2 translated to the COM of c1.
		
		"""
		
		# make sure we don't get bad data
		if len(c1) != len(c2):
			print("Two different length selections, with lengths, %d and %d." % (len(c1), len(c2)))
			print ("This algorithm must be used with selections of the same length.")	
			print ("In PyMol, type 'count_atoms sel1' where sel1 are your selections to find out their lengths.")
			print ("Example:   optAlign MOL1 and i. 20-40, MOL2 and i. 102-122")
        #	print ("Example 2: optAlign 1GGZ and i. 4-146 and n. CA, 1CLL and i. 4-146 and n. CA")

			
		assert len(c1) == len(c2) != 0

		L = len(c1)
		
		#
		# Centers of Mass
		#
		c1COM = numpy.zeros((3,1), numpy.Float64)
		c2COM = numpy.zeros((3,1), numpy.Float64)

		# calculate the CsOM		
		for i in range(L):
			for j in range(3):
				c1COM[j] += c1[i][j]
				c2COM[j] += c2[i][j]
		
		T1 = - c1COM / L
		T2 = - c2COM / L

		# move everything back to the origin.
		for i in range(L):
			for j in range(3):
				c1[i][j] += T1[j]
				c2[i][j] += T2[j]
				
		return T1, T2, c1, c2

					
	def calcR( self, c1, c2 ):
		"""
		Calculate the matrix, R, by (eq7, 1976).  M{r_{i,j} = sum_n w_n * y_{ni} * x_{nj}},
		where M{y_{ni}} is the ith component of the vector M{y_n}.
		(CORRECT)
		
		@param c1: coordinates of the first vectors, as an array of 3D vectors.
		@type  c1: Python list
		   
		@param c2: coordinates of the second set of vectors, as an array of 3D vectors.
		@type  c2: Python list

		@postcondition: R is now defined.
		
		@return: R
		@rtype : 3x3 matrix
				
		"""
		
		# Create the 3x3 matrix
		R = numpy.zeros((3,3), numpy.Float64)
		L = len(c1)
		
		for k in range(L):
			for i in range(3):
				for j in range(3):
					R[i][j] += c2[k][i] * c1[k][j]
		
		# return R the 3x3 PSD Matrix.
		return R
		
	
	def calcRtR( self, R ):
		"""
		Calculate RtR (R-transpose * R).
		(CORRECT)
		
		@param R: Matrix
		@type  R: 3x3 Matrix
		
		@precondition: R is a the well formed matrix as per Kabsch.
		@postcondition: RtR is now defined
		
		@return: M{R^tR}
		@rtype : 3x3 matrix
		
		"""
		
		RtR = numpy.matrixmultiply(numpy.transpose(R), R)
		
		return RtR
	
	
	def calcEigenPairs( self, RtR ):
		"""
		Calculate the corresponding eigenpairs for RtR, and sort them accordingly:
		M{m1 >= m2 >= m3}; set M{v3 = v1 x v2} to ensure a RHS
		(CORRECT)
		
		@postcondition: The eigen pairs are calculated, sorted such that M{m1 >= m2 >= m3} and
		M{v3 = v1 x v2}.
		
		@param RtR: 3x3 Matrix of M{R^t * R}.
		@type  RtR: 3x3 Matrix
		@return : Eigenpairs for the RtR matrix.
		@rtype  : List of stuff
		
		"""
		
		eVal, eVec = LinearAlgebra.eigenvectors(RtR)

		# This is cool.  We sort it using Numeric.sort(eVal)
		# then we reverse it using nifty-crazy ***** notation [::-1].
		eVal2 = numpy.sort(eVal)[::-1]
		eVec2 = [[],[],[]] #Numeric.zeros((3,3), Numeric.Float64)
				
		# Map the vectors to their appropriate owners		
		if eVal2[0] == eVal[0]:
			eVec2[0] = eVec[0]
			if eVal2[1] == eVal[1]:
				eVec2[1] = eVec[1]
				eVec2[2] = eVec[2]
			else:
				eVec2[1] = eVec[2]
				eVec2[2] = eVec[1]
		elif eVal2[0] == eVal[1]:
			eVec2[0] = eVec[1]
			if eVal2[1] == eVal[0]:
				eVec2[1] = eVec[0]
				eVec2[2] = eVec[2]
			else:
				eVec2[1] = eVec[2]
				eVec2[2] = eVec[0]
		elif eVal2[0] == eVal[2]:
			eVec2[0] = eVec[2]
			if eVal2[1] == eVal[1]:
				eVec2[1] = eVec[1]
				eVec2[2] = eVec[0]
			else:
				eVec2[1] = eVec[0]
				eVec2[2] = eVec[1]

		eVec2[2][0] = eVec2[0][1]*eVec2[1][2] - eVec2[0][2]*eVec2[1][1]
		eVec2[2][1] = eVec2[0][2]*eVec2[1][0] - eVec2[0][0]*eVec2[1][2]
		eVec2[2][2] = eVec2[0][0]*eVec2[1][1] - eVec2[0][1]*eVec2[1][0]
		
		return [eVal2, eVec2]
		
	
	def calcBVectors( self, R, eVectors ):
		"""
		Calculate M{R*a_k} and normalize the first two vectors to obtain M{b_1, b_2} and set
		M{b_3 = b_1 x b_2}.  This also takes care of {m2 > m3 = 0}. 
		(CORRECT)
		
		@postcondition: b-Vectors are defined and returned
		
		@return: The three B-vectors
		@rtype: List of 3D vectors (Python LOL).
		"""
		
		bVectors = numpy.zeros((3,3), numpy.Float64)

		for i in range(3):
			bVectors[i] = numpy.matrixmultiply(R, eVectors[i])

		bVectors[0] = bVectors[0] / numpy.sqrt(numpy.add.reduce(bVectors[0]**2))
		bVectors[1] = bVectors[1] / numpy.sqrt(numpy.add.reduce(bVectors[1]**2))
		bVectors[2] = bVectors[2] / numpy.sqrt(numpy.add.reduce(bVectors[2]**2))
		
		bVectors[2][0] = bVectors[0][1]*bVectors[1][2] - bVectors[0][2]*bVectors[1][1]
		bVectors[2][1] = bVectors[0][2]*bVectors[1][0] - bVectors[0][0]*bVectors[1][2]
		bVectors[2][2] = bVectors[0][0]*bVectors[1][1] - bVectors[0][1]*bVectors[1][0]
		
		return bVectors
		
		
		
	def calcU( self, eVectors, bVectors ):
		"""
		Calculate M{U=(u_{ij})=(sum n b_{ki} * a_{kj})} to obtain the best rotation.  Set
		M{sigma_3 = -1 if b3(Ra3) < 0 else sigma_3 = +1}.
		(CORRECT)
		
		@postcondition: U is defined
		
		@param eVectors: Eigenvectors for the system.
		@type  eVectors: Eigenvectors
		
		@param bVectors: BVectors as described by Kabsch.
		@type  bVectors: BVectors
		
		@return: U the rotation matrix.
		@rtype  :3x3 matrix.
		
		"""
		
		U = numpy.zeros((3,3), numpy.Float64)
		
		for k in range(3):
			for i in range(3):
				for j in range(3):
					U[i][j] += numpy.matrixmultiply(bVectors[k][i], eVectors[k][j])
		
		return U
	
		
	def calcE0( self, c1, c2 ):
		"""
		Calculates the initial RMSD, which Kacbsch called E0.
		(CORRECT)
		
		@param c1: coordinats of the first vectors, as an array of 3D vectors.
		@type  c1: Python list
		   
		@param c2: coordinates of the second set of vectors, as an array of 3D vectors.
		@type  c2: Python list
		
		@return: E0 the initial RMSD.
		@rtype : float.
				
		"""
		
		E0 = 0.0
		
		L = len(c1)
		for i in range( L ):
			for j in range(3):
				E0 += 0.5*( c1[i][j]*c1[i][j]+c2[i][j]*c2[i][j])
		
		return E0
	
	def calcRMSD( self, E0, eValues, eVectors, bVectors, R, N):
		"""
		Calculate the RMSD.  The residual error is then
		The M{E = E0 - sqrt(m1) - sqrt(m2) - sigma_3(sqrt(m3))}.
		
		@param E0: Initial RMSD as calculated in L{calcE0}.
		@type  E0: float.
		
		@param eVectors: Eigenvectors as calculated from L{calcEigenPairs}
		@type eVectors: vectors, dammit!
		
		@param bVectors: B-vectors as calc. from L{calcBVectors}
		@type  bVectors: More vectors.
		
		@param R: The matrix R, from L{calcR}.
		@type  R: 3x3 matrix.		

		@param N: Number of equivalenced points
		@type  N: integer
		
		@postcondition: RMSD is computed.
		@return: The RMSD.
		
		"""
		sigma3 = 0
		if numpy.matrixmultiply(bVectors[2], numpy.matrixmultiply( R, eVectors[2])) < 0:
			sigma3 = -1
		else:
			sigma3 = 1

		E = math.sqrt( 2*(E0 - math.sqrt(eValues[0]) - math.sqrt(eValues[1]) - sigma3*(math.sqrt(eValues[2]))) / N)
		
		return E




x = pd.read_csv("mesh_updated.csv", usecols=['X', 'Y', 'Z'])
y = pd.read_csv("mesh_updated.csv", usecols=['X1', 'Y1', 'Z1'])
z = pd.read_csv("mesh_updated.csv", usecols=['comp 2 ID', 'comp1 node ID\'s'])
no_of_rows=len(x.index)
c1=[]
for i in range(no_of_rows):
    lis=[x.iloc[i][0],x.iloc[i][1],x.iloc[i][2]]
    c1.append(lis)
c2=[]
for i in range(no_of_rows):
    lis=[y.iloc[i][0],y.iloc[i][1],y.iloc[i][2]]
    c2.append(lis)
pairs=[]
for i in range(no_of_rows):
    lis=[z.iloc[i][0],z.iloc[i][1]]
    pairs.append(lis)
kabsch=Kabsch()
kabsch.align(c1,c2,pairs)