import numpy

#===============================================================================
"""CLASS"""
#===============================================================================

class Pose(object):
    
    def __init__(self): 
        self.rotationMatrix = None
        self.pattern1 = None
        self.pattern2 = None
        self.score = None
        self.site = None

    def getCrossProductOfVectors(self, v1, v2):
        ortVector = numpy.cross(v1, v2)
        
        return ortVector
    
    def getCenterOfGravity(self): 
        
        sum_x = 0.0
        sum_y = 0.0
        sum_z = 0.0
        
        n = len(self.pattern1+self.pattern2)+1
        
        for point in self.pattern1:
            sum_x = sum_x + point[0]
            sum_y = sum_y + point[1]
            sum_z = sum_z + point[2]
            
        for point in self.pattern2:
            sum_x = sum_x + point[0]
            sum_y = sum_y + point[1]
            sum_z = sum_z + point[2]
                
        gc_x = float(sum_x/n)
        gc_y = float(sum_y/n)
        gc_z = float(sum_z/n)
        
        return [gc_x, gc_y, gc_z]

   
class PoseWithLinearPatterns(Pose):
    
    def getAxesOfRotation(self):
        
        axis1 = self.pattern2[0]
        axis2 = self.findPerpendicular(axis1)
        axis3 = self.getCrossProductOfVectors(axis1, axis2)
        
        return axis1, axis2, axis3

    def getAxis(self):
        axis = self.pattern1[0]
        return axis
    
    def findPerpendicular(self, vect):
        
        a=vect[0]
        b=vect[1]
        c=vect[2]
        
        perp_vect = [1, 1, -(a+b)/c]
        
        return perp_vect

    
class PoseWithPlanarPatterns(Pose):

    def getAxesOfRotation(self):
        
        self.pattern2[0] = numpy.array(self.pattern2[0], dtype=numpy.float64, copy=True)
        self.pattern2[0] = numpy.dot(self.rotationMatrix, self.pattern2[0])
            
        self.pattern2[1] = numpy.array(self.pattern2[1], dtype=numpy.float64, copy=True)
        self.pattern2[1] = numpy.dot(self.rotationMatrix, self.pattern2[1])
            
        axis1 = self.getCenterOfGravity()
            
        vect1 = self.pattern1[0]
        vect2 = self.pattern1[1]
        axis2 = self.getCrossProductOfVectors(vect1, vect2)
            
        axis3 = self.getCrossProductOfVectors(axis1, axis2)

        return axis1, axis2, axis3
