# headtrack-Wiimotelib121-PY24.py

print"""
Rob Salgado (salgado@physics.syr.edu) and Tav Hawkins (taviare@physics.syr.edu)
version June 20, 2008

http://physics.syr.edu/~salgado/software/vpython/
"""

# uses Python 2.4 and WiimoteLib_1.2.1 and pythonnet

from visual import *
from random import *
import CLR           # pythonnet's CLR will call WiimoteLib (see below)


print"""
This [Windows-only] VPython program is an attempt to emulate
the Head Tracking efforts of Johnny Chung Lee
(http://www.cs.cmu.edu/~johnny/projects/wii/).

In order to read in the real-time data from the the Wiimote, 
we use the procedure presented by Nishio Hirokazu in 
http://nishiohirokazu.blogspot.com/2008/01/get-values-from-wii-remote-through.html, 
which uses Brian Peek's (Windows-only) WiimoteLib v1.2.1 
[a library for using a Nintendo Wii Remote (Wiimote) from .NET.]
(http://www.codeplex.com/WiimoteLib/Release/ProjectReleases.aspx?ReleaseId=10283) 
described in "Code4Fun - Managed Library for Nintendo's Wiimote (Brian Peek)"
(http://blogs.msdn.com/coding4fun/archive/2007/03/14/1879033.aspx).
To use .NET with Python, we used Brian Lloyd's Python for .NET 
(http://pythonnet.sourceforge.net/).

Ideally, we would have liked to use the latest versions of 
the various software packages [which should be possible]. 
However, for now, the combination of versions that worked for us 
[with our program] is:

- Python 2.4, VPython 3.2.9
- pythonnet-1.0-rc2-py2.4-clr1.1
- WiimoteLib v1.2.1 

I'm sure there is a platform-independent solution in which 
Python communicates directly with Bluetooth. Of course, 
one now needs to know details of the protocol for talking with the Wiimote. 

Note: This was just thrown together as a "proof of concept". 
The code isn't the prettiest. The tracking and the visualization 
could certainly be improved.

USAGE=========
After pairing your Wiimote with your computer's Bluetooth radio,
your Wiimote can track two IR-leds mounted onto a pair of glasses,
which we use to navigate a scene in VPython. (After seeing how things work,
you may wish to make the scene fullscreen.

For more details, see Johnny Chung Lee's page 
http://www.cs.cmu.edu/~johnny/projects/wii/ 
"""

##based on http://nishiohirokazu.blogspot.com/2008/01/get-values-from-wii-remote-through.html
##
##original from Nishio Hirokazu's article
##
##import clr
##clr.AddReferenceToFile("wiimotelib.dll")

## modified based on
##
## http://mail.python.org/pipermail/pythondotnet/2006-March/000450.html

##import CLR
from CLR.System.Reflection import Assembly

my_dll_name = Assembly.LoadWithPartialName( "WiimoteLib" )

from CLR.WiimoteLib import *

wii = CLR.WiimoteLib.Wiimote()
def get_value(sender, args):
   global a
   a = args

wii.WiimoteChanged += CLR.WiimoteLib.WiimoteChangedEventHandler(get_value)

wii.Connect()
wii.SetReportType(wii.InputReport.IRAccel, True) 

###

WII=a.WiimoteState
WIIset=wii
WIIset.SetLEDs(0,0,0,0)  #setLEDs off off off off (to save battery life)

#####################################################
#####################################################
#####################################################

### for debugging the tracking 
scene2=display(x=600,y=0,autoscale=0,range=(1.5,1.5,1.5),fov=1e-10,forward=(0,0,1),title="mirror image of what the Wiimote sees" )
box(axis=(0,0,.07), width=1,height=1)
box(axis=(0,0,.05), width=2,height=2, color=color.yellow)
IRloc=[]
IRloc.append(sphere(radius=.05,color=color.red))
IRloc.append(sphere(radius=.05,color=color.blue))
scene.select()
###

scene.x=0
scene.y=0
scene.width=600
scene.height=600

#scene.fullscreen=1
scene.autoscale=0
scene.range=(10,10,10)
#scene.stereo='yellowblue'

myIR=[]
myIR.append( sphere(radius=0.2,pos=(0,0,0),color=color.red   ))
myIR.append( sphere(radius=0.2,pos=(0,0,0),color=color.blue  ))


S=20
SZ=10
R=0.1
for j in arange(-S,S,2*S/10):
    curve(pos=[vector(j,-S,S*SZ), vector(j,-S,-S*SZ)],radius=R)
    curve(pos=[vector(j,S,S*SZ), vector(j,S,-S*SZ)],radius=R)
    curve(pos=[vector(-S,j,S*SZ), vector(-S,j,-S*SZ)], radius=R);
    curve(pos=[vector(S,j,S*SZ), vector(S,j,-S*SZ)],radius=R)
    
    curve(pos=[vector(-S,S,j*SZ), vector(S,S,j*SZ)],radius=R)
    curve(pos=[vector(-S,-S,j*SZ), vector(S,-S,j*SZ)],radius=R)
    curve(pos=[vector(-S,-S,j*SZ), vector(-S,S,j*SZ)],radius=R)
    curve(pos=[vector(S,-S,j*SZ), vector(S,S,j*SZ)],radius=R)

    curve(pos=[vector(j,-S,-S*SZ), vector(j,S,-S*SZ)],radius=R)
    curve(pos=[vector(-S,j,-S*SZ), vector(S,j,-S*SZ)],radius=R)


N=25
disk=[]
stick=[]
sq=[]
for i in range(N):
         disk+=[cylinder(axis=vector(0,0,.1),
             pos=(uniform(-S/2.,S/2.),uniform(-S/2.,S/2.),uniform(-S/2.,S/2.)),radius=3*random(),
            color=(random(),random(),random()))]
         disk+=[cylinder(axis=vector(0,0,.2),
             pos=disk[-1].pos,radius=disk[-1].radius*0.75,
            color=( 1-disk[-1].color[0], 1-disk[-1].color[1], 1-disk[-1].color[2] ))]
         stick+=[curve( pos=[ vector(disk[-1].x, disk[-1].y, -S*SZ), disk[-1].pos+vector(0,0,.7) ], radius=disk[-1].radius/5.,
                        color=disk[-1].color )] 



IRscale=2.0
IRcenter= vector(1/2.,1/2.,0)

refD=2

def newEYE(A,B):
    M=(A+B)/2.
    D1=B-A

    if mag(D1)>1.e-1:
        EYEz=refD/mag(D1)
    else:
        EYEz=refD/1.e-1  

    EYEplane=M

    scene.center[2]=EYEz
    scene.forward=vector( EYEplane[0],-EYEplane[1], -1 )


zeroV=vector(0,0,0)



while True:
    rate(50)

    if WII.IRState.Found1:
        myIR[0].pos=IRscale* ( (vector( WII.IRState.X1,WII.IRState.Y1))-IRcenter )
        myIR[0].radius=.01+log(WII.IRState.Size1/100.+1)
        myIR[0].visible=1
    else:
        myIR[0].visible=0
        
    if WII.IRState.Found2:
        myIR[1].pos=IRscale*( (vector( WII.IRState.X2,WII.IRState.Y2))-IRcenter )
        myIR[1].radius=.01+log(WII.IRState.Size2/100.+1)
        myIR[1].visible=1
    else:
        myIR[1].visible=0

    IRloc[0].visible=myIR[0].visible
    IRloc[1].visible=myIR[1].visible

    newEYE(myIR[0].pos, myIR[1].pos)

        
    IRloc[0].pos=myIR[0].pos
    IRloc[1].pos=myIR[1].pos