#!/usr/bin/python
import bluetooth, time
from visual.graph import *
from random import randint

def intsToString(ints) :
	string = ""
	for i in ints :
		string += chr(i)
	return string

def writeRegister(output, addressBytes, dataBytes) :
	message = [0x52] 		# HID
	message.append(0x16) 		# chanel
	message.append(0x04) 		# registers
	message.extend(addressBytes) 	# array extension with address
	message.append(len(dataBytes)) 	# data size
	message.extend(dataBytes) 	# array extension with deta
	message.extend([0 for i in range(16-len(dataBytes))]) # padding
	output.send(intsToString(message))

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

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

def initializeCamera(output) :
	enableCamera(output)
	time.sleep(0.05)
	writeRegister(output, [0xb0, 0x00, 0x30], [8])
	time.sleep(0.05)
	setSensitivity(output)
	time.sleep(0.05)
	writeRegister(output, [0xb0, 0x00, 0x33], [3])
	time.sleep(0.05)
	writeRegister(output, [0xb0, 0x00, 0x30], [8])

def getDot(data, offset) :
	offset *= 3
	offset += 3
	xDown = data[4 + offset]
	yDown = data[5 + offset]
	yUp = (data[6 + offset] & (2**7 + 2**6)) >> 6
	xUp = (data[6 + offset] & (2**5 + 2**4)) >> 4
	size = (data[6 + offset] & (2**3 + 2**2 + 2 + 1))
	x = xDown + xUp * 2**8
	y = yDown + yUp * 2**8
	return ((x,y),size)

def main(input, output) :
	graph1 = gdisplay(x=0, y=0, width=1024, height=768)
	curves = []
	curves.append(gcurve(color=color.white))
	output.send(chr(0x52) + chr(0x12) + chr(0x00) + chr(0x33))
	try :
		offTimes = 0
		initializeCamera(output)
		while True :
			charData = input.recv(23)
			intData = [ ord(char) for char in charData ]
			if (intData[3] & 0x08) == 8 :
				return
			if len(intData) > 9 :
				dot1 = getDot(intData, 0)
				dot2 = getDot(intData, 1)
				if dot1[0][0] > 0 and dot1[0][1] > 0 and dot1[0][0] < 1024 and dot1[0][1] < 768 :
					curves[len(curves)-1].plot(pos=(dot1[0][0],-dot1[0][1]))
					offTimes = 0
				else :
					offTimes += 1
				if offTimes >= 100 :
					r = randint(0,255)*1.0/255
					g = randint(0,255)*1.0/255
					b = randint(0,255)*1.0/255
					curves.append(gcurve(color=(r, g, b)))
					print "curve reset"
					offTimes = 0

					
	except bluetooth.BluetoothError, inst :
		print "bluetooth error : " + str(inst)
	
def getWiimoteAddress() :
	fileHandle = open("wiimote.mac", 'r')
	wiimoteAddress = fileHandle.read()
	fileHandle.close()
	return wiimoteAddress

def connect() :
	wiimoteAddress=getWiimoteAddress()
	input = bluetooth.BluetoothSocket(bluetooth.L2CAP)
	output = bluetooth.BluetoothSocket(bluetooth.L2CAP)
	input.connect((wiimoteAddress, 0x13))
	output.connect((wiimoteAddress, 0x11))
	if input and output :
		print "connexion"
		main(input, output)
	else :
		print "connexion rejetee"
		return False

connect()