from bge import logic as gl
from mathutils import *
from math import *
from vec import *
from cmath import phase

x, y, z = 0, 1, 2


cont = gl.getCurrentController()
scene = gl.getCurrentScene()
own = cont.owner

col = cont.sensors["Col"]

if col.positive and own["active"] :
	obj = col.hitObject
	parent = obj.parent
	
	if not parent == None :
		parent = parent.name
	else : parent = ""
	
	if not obj["prec"] == int(own["bleu"]) and not parent == "EmptyTir" :
		object_list = scene.objects
		
		if own["bleu"] :
			portail = object_list["PortailOrange"]
		else :
			portail = object_list["PortailBleu"]
		
		if portail["active"] :
			rotObj = obj.worldOrientation
			rotOwn = own.worldOrientation
			rotPortail = portail.worldOrientation
			force = obj.worldLinearVelocity
			
			# Deplacement
			obj.position = portail.position	+ rotPortail[z]
			
			# Rotation
			if -0.7 < rotPortail[z][z] < 0.7 and -0.7 < rotOwn[z][z] < 0.7 :
				# Calcul de l'angle entre le 1er portail et le joueur
				angle = phase(complex(-rotObj[y][x], -rotObj[y][y]))\
					 - phase(complex(rotOwn[z][x], rotOwn[z][y]))
				
				# Calcul de l'angle entre le 2nd portail et le joueur
				objYrot = phase(complex(rotPortail[z][x], rotPortail[z][y])) - angle
				
				obj.orientation = [[cos(objYrot - pi/2), sin(objYrot - pi/2), 0.], [cos(objYrot), sin(objYrot), 0.], [0., 0., 1.]]
			
			# Transmition de la force
			if -0.7 < rotPortail[z][z] < 0.7 and -0.7 < rotOwn[z][z] < 0.7 :
				modForce = (force[x]**2 + force[y]**2 + force[z]**2)**0.5
				
				angle = phase(complex(-force[x], -force[y]))\
					 - phase(complex(rotOwn[z][x], rotOwn[z][y]))
				
				forceApp = phase(complex(rotPortail[z][x], rotPortail[z][y])) - angle
				
				obj.worldLinearVelocity = [cos(forceApp)*modForce, sin(forceApp)*modForce, force[z]]
			
			obj["prec"] = abs(int(own["bleu"]) - 1)