#!/usr/bin/python

# Wiibrator - a WiiMote / TranceVibrator interface
#
# Copyright (c) 2006 Sam Hocevar <sam@zoy.org>
#
#    This program is free software. It comes without any warranty, to
#    the extent permitted by applicable law. You can redistribute it
#    and/or modify it under the terms of the Do What The Fuck You Want
#    To Public License, Version 2, as published by Sam Hocevar. See
#    http://sam.zoy.org/wtfpl/COPYING for more details.

from bluetooth import BluetoothSocket, BluetoothError, L2CAP
from FFT import fft
import thread, time

WIIMOTE = '00:19:1D:88:56:BA' # Change this one, of course
VIBRATOR = '/sys/bus/usb/drivers/trancevibrator/2-2:1.0/speed' # Change this, too

FFTSIZE = 32

# Listening thread
def listener():
    global connected, sensor
    btin.settimeout(0.1)
    while connected == 1:
        try:
            msg = btin.recv(23)
        except BluetoothError:
            continue
        if len(msg) >= 7:
            for c in range(3):
                sensor[c] = ord(msg[4 + c])
    btin.close()
    btout.close()
    connected = -1

# Try to connect to Trancevibrator
print 'connecting to Trancevibrator ' + VIBRATOR
tvout = open(VIBRATOR, "w")
tvout.write("0\n")
tvout.flush()
if not tvout:
    raise 'could not open Trancevibrator'
# Try to connect to Wiimote
print 'connecting to Wiimote ' + WIIMOTE + ', please press buttons 1 and 2'
btin = BluetoothSocket(L2CAP)
btin.connect((WIIMOTE, 0x13))
btout = BluetoothSocket(L2CAP)
btout.connect((WIIMOTE, 0x11))
if not btin or not btout:
    raise 'could not connect to Wiimote'
# Initialise data
print 'connected!'
sensor = [127] * 3
freq = [0] * 3
speed = 0
newspeed = 0
data = [[]] * 3
for c in range(3):
    data[c] = [127] * FFTSIZE
# Run listener
connected = 1
thread.start_new_thread(listener, ())
btout.send("\x52\x12\x00\x31")
# Main display loop
count = 0
while connected == 1:
    tab = []
    for c in range(3):
        del data[c][0]
        data[c].append(sensor[c])
    if count > 15:
        for c in range(3):
            freq[c] = (freq[c] + abs((fft(data[c]).real)[1])) / 2
        newspeed = max(0, freq[1] - freq[0] - freq[2])
        count = 0
    speed = (speed * 7 + newspeed) / 8
    tvout.write(str(int(speed)) + "\n")
    tvout.flush()
    time.sleep(0.01)
    count += 1
while connected == 0:
    time.sleep(0.01)