#!/usr/bin/python
import bluetooth
import threading
import time
import sys
import wave

from threading import Thread
from visual import *

wiimote_adress="00:21:BD:01:68:92"

receiveSocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)
controlSocket = bluetooth.BluetoothSocket(bluetooth.L2CAP)

etat = "deconnecte"

def connect() :
	global etat
	receiveSocket.connect((wiimote_adress, 0x13))
	controlSocket.connect((wiimote_adress, 0x11))
	if receiveSocket and controlSocket :
		etat = "connecte"
		wiiThread = threading.Thread(None, receive, None, (1,), None)
		wiiThread.start()
		return True
	else :
		print "connexion rejetee"
		return False

shootPos = 0
def actionsOnButtons(etat, data, shoot, position) :
	global shootPos
	octet1 = ord(data[2])
	octet2 = ord(data[3])
	if (octet1 & 0x01) == 1 :
		print "pad gauche"
	if (octet1 & 0x02) == 2 :
		print "pad droit"
	if (octet1 & 0x04) == 4 :
		print "pad bas : playing increment"
	if (octet1 & 0x08) == 8 :
		print "pad haut : playing sound from file"
	if (octet1 & 0x10) == 0x10 :
		print "plus"
	if (octet2 & 0x01) == 1 :
		print "2"
	if (octet2 & 0x02) == 2 :
		print "1"
	if (octet2 & 0x04) == 4 :
		print "B"
		if shootPos >= 0 :
			shootPos -= 1
			
	if (octet2 & 0x08) == 8 :
		print "deconnection"
		etat = "deconnecte"
	if shootPos < 0 :
		shootPos -= 1
	if shootPos < -200 :
		shootPos = 0
	shoot.pos = - position.pos
	shoot.pos[2] = shootPos - position.pos[2]

	return etat

def actionsOnMode30(etat) :
	while etat == "connecte" :
		try :
			data = receiveSocket.recv(23)
			chaine = ""
			if len(data) == 4 :
				etat = actionsOnButtons(etat, data)

		except bluetooth.BluetoothError :
			pass
	receiveSocket.close()
	controlSocket.close()
	etat = "deconnecte"
	return
def intsToString(ints) :
	string = ""
	for i in ints :
		string += chr(i) 
	return string

def intsToHexString(ints) :
	string = "hex "
	for i in ints :
		string += str(hex(i)) + " "
	return string
	

def writeRegister(adress, size, data) :
	message = [0x52]
	# read
	message.append(0x16)
	# registers
	message.append(0x04)
	# adress
	message.extend(adress)
	# size
	message.append(size)
	# message
	message.extend(data)
	message.extend([0 for i in range(16-size)])
	controlSocket.send(intsToString(message))
	# print intsToHexString(message)
	# printAcknoledgment()

def askForRegisterValue(adress, size) :
	message = [0x52]
	# read
	message.append(0x17)
	# registers
	message.append(0x04)
	# adress
	message.extend(adress)
	# size
	message.extend(size)
	print intsToHexString(message)
	controlSocket.send(intsToString(message))

def enableCamera() :
	controlSocket.send(chr(0x52) + chr(0x13) + chr(0x04))
	controlSocket.send(chr(0x52) + chr(0x1a) + chr(0x04))

def setSensitivity() :
	writeRegister([0xb0, 0x00, 0x00], 9, [0, 0, 0, 0, 0, 0, 0x90, 0, 0xc0])
	writeRegister([0xb0, 0x00, 0x1a], 2, [0x40, 0])

def initializeCamera(mode=3) :
	print "initializing camera"
	enableCamera()
	time.sleep(0.05)
	writeRegister([0xb0, 0x00, 0x30], 1, [8])
	time.sleep(0.05)
	setSensitivity()
	time.sleep(0.05)
	writeRegister([0xb0, 0x00, 0x33], 1, [mode])
	time.sleep(0.05)
	writeRegister([0xb0, 0x00, 0x30], 1, [8])
	print "camera initialized"

def getDot(data, offset) :
	offset *= 3
	# we dont care about accelerometer data
	offset += 3
	x1Bas = ord(data[4 + offset])
	y1Bas = ord(data[5 + offset])

	y1Haut = (ord(data[6 + offset]) & (2**7 + 2**6)) >> 6
	x1Haut = (ord(data[6 + offset]) & (2**5 + 2**4)) >> 4
	size = (ord(data[6 + offset]) & (2**3 + 2**2 + 2 + 1))

	x = x1Bas + x1Haut * 2**8
	y = y1Bas + y1Haut * 2**8
	return ((x,y),size)
lastangle = 0
def IRActions(data, p1, p2, position, scene) :
	global lastangle
	((x1,y1),dot1Size) = getDot(data, 0)
	((x2,y2),dot2Size) = getDot(data, 1)

	# print "(" + str(x1) + ", " + str(y1) + "), " + "(" + str(x2) + ", " + str(y2) + ")"
	if (x1 < 1023 and y1 < 768) and (x2 < 1023 and y2 < 768) :
		dx = (x1 - x2) *  1.0 / 1024 ;
		dy = (y1 - y2) * 1.0 / 768;
		fov = math.pi / 4
		barWidth = 200.0
		dist = sqrt(dx * dx + dy * dy);
		angl = fov * dist / 2.0;
		if angl != 0 :
			headDist = (barWidth / 2.0) / tan(angl);
			mx = (x2 + x1) / 1024.0 / 2.0;
			my = (y2 + y1) / 768.0 / 2.0;
			rx = sin(fov * mx) * headDist * 0.5;
			ry = sin(fov * my) * headDist * 1.5;
			fx = -rx;
			fy = -ry;
			fz = headDist;
			#position.pos[0] = -fx/10 
			relx = (x1 + (x2 - x1) / 2 - 512) * (barWidth / (x2 - x1))
			angl = ((dot2Size - dot1Size) / 30) * fov;
			# position.rotate(angle=-lastangle, axis=(0,1,0), origin=position.pos)
			position.pos[2] = 20 -fz/10 
			position.pos[1] = -10 -fy/100 
			position.pos[0] = -20 -fx/10
			print "angle " + str(angl * 180.0 / math.pi)
			angle2 = angl
			if (fx > 0) :
				angle2 = -angle2 

			# position.rotate(angle=angle2, axis=(0,1,0), origin=position.pos)
			lastangle = angle2


def actionsOnMode36(etat, mode) :
	scene = display(title='Graph of position',width=750, height=750, background=(0,0,0))
	scene.autocenter = 0
	scene.autoscale = 0
	p1 = 0 
	p2 = 0 
	position = frame(pos=(0, 0, 0))
	box(frame=position, pos=(5, 0, 0), length=10, width=10, height=10, color=color.green)
	box(frame=position, pos=(-10, 0, 15), length=10, width=10, height=10, color=color.blue)
	box(frame=position, pos=(10, 0, 25), length=10, width=10, height=10, color=color.red)
	box(frame=position, pos=(-15, 0, 35), length=10, width=10, height=10, color=color.yellow)
	box(frame=position, pos=(20, 0, 45), length=10, width=10, height=10, color=(0.1, 0.1, 0))
	box(frame=position, pos=(-20, 0, 50), length=10, width=10, height=10, color=(0.1, 0.9, 0))
	box(frame=position, pos=(0, -5.2, 0), length=100, width=300, height=0.4, color=(0.7, 0.7, 0.7))
	box(frame=position, pos=(-30, 5, 60), length=5, width=10, height=20, color=color.white)
	box(frame=position, pos=(30, 5, 60), length=5, width=10, height=20, color=color.white)
	box(frame=position, pos=(0, 20, 60), length=60, width=10, height=10, color=color.white)
	shoot=box(pos=(0,0,0), length=5, width=10, height=5, color=color.green)
	try :
		initializeCamera()
		while etat == "connecte" :
			data = receiveSocket.recv(23)
			etat = actionsOnButtons(etat, data, shoot, position)
			if len(data) > 9 :
				IRActions(data, p1, p2, position, scene)
	except bluetooth.BluetoothError, inst :
		print "bluetooth error"
		print inst
		pass
	receiveSocket.close()
	controlSocket.close()
	etat = "deconnecte"
	return

def receive(nb) :
	global etat
	global mode
	#receiveSocket.settimeout(2)
	controlSocket.send(chr(0x52) + chr(0x12) + chr(0x00) + chr(0x33))
	return actionsOnMode36(etat, mode)

mode = 0x36
if mode :
	connect()