#!/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
def actionsOnButtons(etat, data) :
	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 : playing little sound"
	if (octet2 & 0x08) == 8 :
		print "deconnection"
		etat = "deconnecte"
	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() :
	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, [1])
	time.sleep(0.05)
	writeRegister([0xb0, 0x00, 0x30], 1, [8])
	print "camera initialized"

def IRActions(data, p1, p2) :
	x1Bas = ord(data[4])
	y1Bas = ord(data[5])
	x2Bas = ord(data[7])
	y2Bas = ord(data[8])

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

	x1 = x1Bas + x1Haut * 2**8
	x2 = x2Bas + x2Haut * 2**8
	y1 = y1Bas + y1Haut * 2**8
	y2 = y2Bas + y2Haut * 2**8

	print "(" + str(x1) + ", " + str(y1) + "), " + "(" + str(x2) + ", " + str(y2) + ")"
	if x1 < 1024 and y1 < 768 :
		p1.pos[0] = x1
		p1.pos[1] = y1
	if x2 < 1024 and y2 < 768 :
		p2.pos[0] = x2
		p2.pos[1] = y2
	
	

def actionsOnMode36(etat, mode) :
	scene2 = display(title='Graph of position',width=750, height=750, background=(0,0,0))
	box(pos=(1024/2, 768/2,-200), length=1024, width=0.1, height=768, color=(0.6,0.6,0.6))
	p1 = sphere(pos=(1024/2, 768/2, -200), radius=10, color=color.blue)
	p2 = sphere(pos=(1024/2, 768/2, -200), radius=10, color=color.red)
	try :
		initializeCamera()
		while etat == "connecte" :
			data = receiveSocket.recv(23)
			etat = actionsOnButtons(etat, data)
			if len(data) > 9 :
				IRActions(data, p1, p2)
	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(0x36))
	return actionsOnMode36(etat, mode)

mode = 0x36
if mode :
	connect()